APP_Framework/Framework/:complete one order ordinary kalman filter.

APP_Framework/Framework/knowing/filter/kalman_filter/one_order_kalman.c

@@ -1 +1,29 @@
-#include <one_order_kalman.h>
+#include <one_order_kalman.h>
+/**
+ *      equation of state       x = A*x + w; w~N(0,Q)
+ *      Measurement equation    y = C*x + v; v~N(0,R)
+ * 
+ */
+
+void OneOrderKalmamInit(OneOrderKalmanHander *hander,float A,float C,float Q ,float R)
+{
+    hander->A = A;
+	hander->A2 = A * A;
+	hander->C = C;
+	hander->C2 = C * C;
+	hander->Q = Q;
+	hander->R = R;
+	hander->x = 0;
+	hander->P = Q;
+	hander->K = 1;
+}
+
+float OneOrderKalmanFun(OneOrderKalmanHander *hander,float measure)
+{
+    hander->x = hander->A * hander->x;                                               //state prediction 
+    hander->P = hander->A2 * hander->P + hander->Q;                                  //covariance prediction
+    hander->K = hander->P * hander->C / (hander->C2 * hander->P + hander->R);        //calculate kalman gain
+    hander->x = hander->x + hander->K * (measure- hander->C * hander->x);            //state correct
+    hander->P = (1 - hander->K * hander->C) * hander->P;                             //covariance correction
+    return hander->C * hander->x; 	                                                 //return resault   
+}

APP_Framework/Framework/knowing/filter/kalman_filter/one_order_kalman.h

@@ -1,4 +1,23 @@
 #ifndef _ONE_ORDER_KALMAN_H_
 #define _ONE_ORDER_KALMAN_H_
+/**
+ *      equation of state       x = A*x + w; w~N(0,Q)
+ *      Measurement equation    y = C*x + v; v~N(0,R)
+ * 
+ */
+typedef struct 
+{
+    float A;
+    float C;
+	float A2;  //A*A
+    float C2;  //C*C
+	float Q;
+    float R;
+    float K;
+	float P;
+	float x;
+}OneOrderKalmanHander;
 
+void OneOrderKalmamInit(OneOrderKalmanHander *hander,float A,float C,float Q ,float R);
+float OneOrderKalmanFun(OneOrderKalmanHander *hander,float measure);
 #endif