Diff: FILTER_LIB.h

Revision:

7:10df955a92d9

Parent:

6:18dd3f9ac217

--- 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