forked from xuos/xiuos
APP_Framework/Framework/:complete one order ordinary kalman filter.
This commit is contained in:
parent
d16c698789
commit
41b8b3e896
|
@ -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
|
||||
}
|
|
@ -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
|
Loading…
Reference in New Issue