卡尔曼滤波算法c语言stm32,STM32怎么实现自适应卡尔曼滤波?

主要是C语言实现的算法/@@*-------------------------------------------------------------------------------------------------------------*/

/@@*

Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏

R:测量噪声，R增大，动态响应变慢，收敛稳定性变好

*/

double KalmanFilter(const double ResrcData

double ProcessNiose_Qdouble MeasureNoise_Rdouble InitialPrediction)

{

double R = MeasureNoise_R;

double Q = ProcessNiose_Q;

static double x_last;

double x_mid = x_last;

double x_now;

static double p_last;

double p_mid ;

double p_now;

double kg;

x_mid=x_last; //x_last=x(k-1|k-1)x_mid=x(k|k-1)

p_mid=p_last+Q; //p_mid=p(k|k-1)p_last=p(k-1|k-1)Q=噪声

kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声

x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值

p_now=(1-kg)*p_mid;//最优值对应的covariance

p_last = p_now; //更新covariance值

x_last = x_now; //更新系统状态值

return x_now;

}

/@@*-------------------------------------------------------------------------------------------------------------*/