Revision 7:10df955a92d9, committed 2017-02-10

Comitter:

benson516

Date:

Fri Feb 10 18:26:47 2017 +0000

Parent:

6:18dd3f9ac217

Commit message:

Add the first-order Kalman filter

Changed in this revision

diff -r 18dd3f9ac217 -r 10df955a92d9 FILTER_LIB.cpp
--- a/FILTER_LIB.cpp	Fri Jan 20 15:49:04 2017 +0000
+++ b/FILTER_LIB.cpp	Fri Feb 10 18:26:47 2017 +0000
@@ -63,10 +63,10 @@
     }
     //
     static vector<float>::iterator it_output;
-    static vector<const float>::iterator it_input;
+    static vector<float>::iterator it_input;
     //
     it_output = output.begin();
-    it_input = input.begin();
+    it_input = vector<float>(input).begin();
     for (size_t i = 0; i < n; ++i){
         // output = One_alpha_Ts*output + alpha_Ts*input;
         *it_output += alpha_Ts*(*it_input - *it_output);
@@ -260,11 +260,11 @@
 
     // hpf = (1 - lpf)*x
     static vector<float>::iterator it_output;
-    static vector<const float>::iterator it_input;
+    static vector<float>::iterator it_input;
     static vector<float>::iterator it_lpf_v;
     //
     it_output = output.begin();
-    it_input = input.begin();
+    it_input = vector<float>(input).begin();
     it_lpf_v = lpf_v.output.begin();
     for (size_t i = 0; i < n; ++i){
         *it_output = *it_input - *it_lpf_v;
@@ -430,11 +430,11 @@
 
     // hpf = (1 - lpf)*x
     static vector<float>::iterator it_output;
-    static vector<const float>::iterator it_input;
+    static vector<float>::iterator it_input;
     static vector<float>::iterator it_lastFilter;
     //
     it_output = output.begin();
-    it_input = input.begin();
+    it_input = vector<float>(input).begin();
     it_lastFilter = filter_layers[order-1].output.begin();
     for (size_t i = 0; i < n; ++i){
         *it_output = *it_input - *it_lastFilter;
@@ -486,7 +486,7 @@
     float num[] = {44.3396241975210,    88.6792483950420,   0., -88.6792483950420,  -44.3396241975210};
     float den[] = {1.,  -0.913043639239699, 0.312618257683527,  -0.0475723519480234,    0.00271472708436336};
     float gain = 1.0;
-    
+
 
     /*
     // 20% of Fs, 6th-order LPF
@@ -565,3 +565,168 @@
     output = input;
     return;
 }
+
+//-----------First-Order Kalman Filter--------//
+// FirstOrder_KalmanFilter
+FirstOrder_KalmanFilter::FirstOrder_KalmanFilter(float samplingTime, float A_in, float B_in, float C_in, float R_in, float Q_in, bool is_continuousTime){ // If is_continuousTime -> continuous time system
+    //
+    Ts = samplingTime;
+
+    // Parameters
+    if(is_continuousTime){
+        A = 1.0 + Ts*A_in;
+        B = Ts*B_in;
+    }else{
+        A = A_in;
+        B = B_in;
+    }
+    //
+    C = C_in;
+    //
+    R = R_in;
+    Q = Q_in;
+
+
+    //
+    mu_est = 0.0;
+    Sigma_est = 100000; // No a-priori knoledge
+
+    // Kalman gain
+    K = 0;
+
+    //
+    Flag_Init = false;
+}
+float FirstOrder_KalmanFilter::filter(float u, float z){
+    // Initialization
+    if (!Flag_Init){
+        reset(z);
+        Flag_Init = true;
+        return mu_est;
+    }
+
+    // Prediction
+    mu_est  = A*mu_est + B*u;
+    Sigma_est = A*Sigma_est*A + R;
+    // Update
+    K = Sigma_est*C/(C*Sigma_est*C + Q);
+    mu_est += K*(z - C*mu_est);
+    Sigma_est -= K*C*Sigma_est;
+    //
+
+    return mu_est;
+}
+void FirstOrder_KalmanFilter::reset(float z){
+    //
+    mu_est = z;
+    Sigma_est = 100000; // No a-priori knoledge
+
+    // Kalman gain
+    K = 0;
+    return;
+}
+
+//-----------Saturation--------//
+// Saturation
+Saturation::Saturation(float bound_up_in, float bound_low_in){ // If is_continuousTime -> continuous time system
+
+    //
+    bound_up = bound_up_in;
+    bound_low = bound_low_in;
+
+    //
+    output = 0.0;
+
+
+    Flag_Init = false;
+}
+float Saturation::filter(float input){
+    // Initialization
+    if (!Flag_Init){
+        reset(input);
+        Flag_Init = true;
+        return output;
+    }
+
+    // Saturation
+    if (input > bound_up){
+        output = bound_up;
+    }else if (input < bound_low){
+        output = bound_low;
+    }else{
+        output = input;
+    }
+
+    return output;
+}
+void Saturation::reset(float input){
+    //
+    // Saturation
+    if (input > bound_up){
+        output = bound_up;
+    }else if (input < bound_low){
+        output = bound_low;
+    }else{
+        output = input;
+    }
+
+    return;
+}
+
+//-----------Saturation_vector--------//
+// Saturation_vector
+Saturation_vector::Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in){ // If is_continuousTime -> continuous time system
+    //
+    n = dimension;
+
+    //
+    bound_up = bound_up_in;
+    bound_low = bound_low_in;
+
+    //
+    output.assign(n, 0.0);
+
+
+    Flag_Init = false;
+}
+vector<float> Saturation_vector::filter(vector<float> input){
+    // Initialization
+    if (!Flag_Init){
+        reset(input);
+        Flag_Init = true;
+        return output;
+    }
+
+    // Saturation
+    for (size_t i = 0; i < n; ++i){
+        //
+        if (input[i] > bound_up){
+            output[i] = bound_up;
+        }else if (input[i] < bound_low){
+            output[i] = bound_low;
+        }else{
+            output[i] = input[i];
+        }
+        //
+    }
+
+
+    return output;
+}
+void Saturation_vector::reset(vector<float> input){
+    //
+    // Saturation
+    for (size_t i = 0; i < n; ++i){
+        //
+        if (input[i] > bound_up){
+            output[i] = bound_up;
+        }else if (input[i] < bound_low){
+            output[i] = bound_low;
+        }else{
+            output[i] = input[i];
+        }
+        //
+    }
+
+    return;
+}

diff -r 18dd3f9ac217 -r 10df955a92d9 FILTER_LIB.h
--- a/FILTER_LIB.h	Fri Jan 20 15:49:04 2017 +0000
+++ b/FILTER_LIB.h	Fri Feb 10 18:26:47 2017 +0000
@@ -261,4 +261,78 @@
     bool Flag_Init;
 };
 
+//-----------First-Order Kalman Filter--------//
+class FirstOrder_KalmanFilter{ // 1st-order Kalman filter
+public:
+
+    // Parameters
+    float A;
+    float B;
+    float C;
+    //
+    float R;
+    float Q;
+
+    // States
+    float mu_est;
+    float Sigma_est;
+    // Kalman gain
+    float K;
+
+    FirstOrder_KalmanFilter(float samplingTime, float A_in, float B_in, float C_in, float R_in, float Q_in, bool is_continuousTime); // If is_continuousTime -> continuous time system
+    float filter(float u, float z);
+    void reset(float z);
+
+private:
+    float Ts;
+
+
+    // Flag
+    bool Flag_Init;
+};
+
+//-----------------Saturation---------------//
+class Saturation{ // Saturation
+public:
+
+    // States
+    float output;
+
+    Saturation(float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
+    float filter(float input);
+    void reset(float input);
+
+private:
+    float Ts;
+
+    //
+    float bound_up;
+    float bound_low;
+
+    // Flag
+    bool Flag_Init;
+};
+
+//-----------------Saturation_vector---------------//
+class Saturation_vector{ // Saturation
+public:
+
+    // States
+    vector<float> output;
+
+    Saturation_vector(size_t dimension, float bound_up_in, float bound_low_in); // If is_continuousTime -> continuous time system
+    vector<float> filter(vector<float> input);
+    void reset(vector<float> input);
+
+private:
+    size_t n;
+
+    //
+    float bound_up;
+    float bound_low;
+
+    // Flag
+    bool Flag_Init;
+};
+
 #endif