34 lines
879 B
C
34 lines
879 B
C
#include "kalman.h"
|
||
|
||
|
||
|
||
int kalman_init(kalman_struct *k)
|
||
{
|
||
k->x_last=0;
|
||
k->p_last=0.02;
|
||
k->Q=0.018;
|
||
k->R=0.542;
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
float kalman_filter(kalman_struct *k,float now)
|
||
{
|
||
k->x_mid=k->x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
|
||
k->p_mid=k->p_last+k->Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q= 噪声
|
||
k->kg=k->p_mid/(k->p_mid+k->R); //kg 为kalman filter ，R为噪声
|
||
// k->z_measure=z_real+frand()*0.03; // 测量值
|
||
k->x_now=k->x_mid+k->kg*(now-k->x_mid); // 估计出的最优值
|
||
k->p_now=(1-k->kg)*k->p_mid; // 最优值对应的covariance
|
||
|
||
k->p_last = k->p_now; // 更新covariance 值
|
||
k->x_last = k->x_now; // 更新系统状态值
|
||
|
||
return k->x_now;
|
||
}
|
||
|
||
|
||
|
||
|