卡尔曼滤波算法C语言实现

图1 真值为0.56的运行结果

给定值z_real=3.32时的运行结果如图2

图2 真值为3.32的运行结果

图3为Q，R不变，p_last=0.02，x_last=0，z_real=0.56时的测试结果。通过和前两次结果比较发现p_last对估计结果影响较大，分析可知这种现象是符合kalman filter的，通过改变Q和R的值也能改善算法的性能，但是实际操作中我们并不能控制这两个量。

5

图3 改变p_last的测试结果

6 结论

本文通过对kalman filter algorithm的深入探讨，对kalman filter有了更深刻的认识，理解了核心的5条公式的物理意义，以及kalman filter的思想，并通过理解算法编程实践，验证了kalman filter在数据处理方面的优良性能。

并且通过实验结果分析了kalman filter algorithm的本质对每次估计产生的协方差递归结合当前测量值来估计系统当前的最佳状态。如要改善算法的性能就必须要尽可能的减小系统噪声和测量噪声，优化程序，减小估计的协方差。

参考文献

[1]谭浩强.C程序设计(第三版)[M].北京:清华大学出版社,2005,91~130.

[2]崔平远,黄晓瑞.基于联合卡尔曼滤波的多传感器信息融合算法及其应用[J].电机与控制学报,2001,9(5): 204-207.

[3]党宏社,韩崇昭,段战胜.基于多卡尔曼滤波器的自适应传感器融合[J].系统工程与电子技术,2004,5(26):311-313.

[4]文贡坚,王润生. 一种稳健的直线提取算法[J].软件学报,2001,11(11):1660-1665.

6

附录：源程序

#include \#include \#include \double frand()

{ return 2*((rand()/(double)RAND_MAX) - 0.5); //随机噪声} void main() {

float x_last=0; float p_last=0.02; float Q=0.018; float R=0.542; float kg; float x_mid; float x_now; float p_mid; float p_now;

float z_real=0.56;//0.56 float z_measure; float sumerror_kalman=0; float sumerror_measure=0; int i;

x_last=z_real+frand()*0.03; x_mid=x_last; for(i=0;i<20;i++)

{ 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为噪声 z_measure=z_real+frand()*0.03;//测量值

x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值 p_now=(1-kg)*p_mid;//最优值对应的covariance

printf(\显示真值

printf(\position: %6.3f [diff:%.3f]\\n\//显示测量值以及真值与测量值之间的误差

printf(\显示kalman估计值以及真值和卡尔曼估计值的误差

sumerror_kalman += fabs(z_real - x_now); //kalman估计值的累积误差 sumerror_measure += fabs(z_real-z_measure); //真值与测量值的累积误差 p_last = p_now; //更新covariance值 x_last = x_now; //更新系统状态值 }

7

}

printf(\总体测量误差 : %f\\n\输出测量累积误差 printf(\总体卡尔曼滤波误差: %f\\n\输出kalman累积误差

printf(\卡尔曼误差所占比例: %d%% \\n\