2025-06-27 00:32:57 +08:00
|
|
|
|
#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)
|
2025-07-05 19:47:28 +08:00
|
|
|
|
k->p_mid=k->p_last+k->Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q= <20><><EFBFBD><EFBFBD>
|
|
|
|
|
|
k->kg=k->p_mid/(k->p_mid+k->R); //kg Ϊkalman filter <20><>RΪ<52><CEAA><EFBFBD><EFBFBD>
|
|
|
|
|
|
// k->z_measure=z_real+frand()*0.03; // <20><><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
k->x_now=k->x_mid+k->kg*(now-k->x_mid); // <20><><EFBFBD>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
k->p_now=(1-k->kg)*k->p_mid; // <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ӧ<EFBFBD><D3A6>covariance
|
|
|
|
|
|
|
|
|
|
|
|
k->p_last = k->p_now; // <20><><EFBFBD><EFBFBD>covariance ֵ
|
|
|
|
|
|
k->x_last = k->x_now; // <20><><EFBFBD><EFBFBD>ϵͳ״ֵ̬
|
2025-06-27 00:32:57 +08:00
|
|
|
|
|
|
|
|
|
|
return k->x_now;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
