autotest
/
xiuos
forked from xuos/xiuos
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

This commit is contained in:
chunyexixiaoyu 2021-08-19 18:10:40 +08:00
parent d16c698789
commit 41b8b3e896
2 changed files with 48 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
APP_Framework/Framework/knowing/filter/kalman_filter/one_order_kalman.c
View File

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

19
APP_Framework/Framework/knowing/filter/kalman_filter/one_order_kalman.h
View File

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