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

1卡尔曼滤波算法及C语言实现摘要：本文着重讨论了卡尔曼滤波器的原理，典型算法以及应用领域。清晰地阐述了kalmanfilter在信息估计方面的最优性能。着重介绍简单kalmanfilteralgorithm的编程，使用kalmanfilter的经典5个体现最优化递归公式来编程。通过c语言编写程序实现kalmanfilter的最优估计能力。关键词：kalmanfilter；最优估计；C语言1引言KalmanFilter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。起源于RudolfEmilKalman在1960年的博士论文和发表的论文《ANewApproachtoLinearEilteringandPredictionProblems》（《线性滤波与预测问题的新方法》）。并且最先在阿波罗登月计划轨迹预测上应用成功，此后kalmanfilter取得重大发展和完善。它的广泛应用已经超过30年，包括机器人导航，控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，近年来更被广泛应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。2kalmanfilter最优化递归估计Kalmanfilter是一个“optimalrecursivedataprocessingalgorithm（最优化递归数据处理方法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的方法。而kalmanfilter最为核心的内容是体现它最优化估计和递归特点的5条公式。举一个例子来详细说明5条公式的物理意义。假设我们要研究的对象是某一个房间的温度信号。对于室温来说，一分钟内或一小段时间内的值是基本上不变的或者变化范围很小。也就是说1t时刻的温度1T和2t时刻的温度2T基本不变，即12TT。在这个过程中，因为毕竟温度还是有所改变的，设有几度的偏差。我们把这几度的偏差看成是高斯白噪声)(tw，也就是说0)]([twE，2)]([twD。除此之外我们在用一个温度计来实时测量房间的温度值Z，但由于量具本身的误差，所测得的温度值也是不准确的，也会和实际值偏差几度，把这几度的偏差看成是测量噪声)(tv。即满足0)]([tvE，21)]([tvD。此时我们对于这个房间的温度就得到了两个数据。一个是你根据经验得到的经验值212TT，一个是从温度计上得到的测量值Z，以及各自引入的高斯白噪声。下面就具体讲解kalmanfilter来估计房间温度的原理与步骤。要估计K时刻的实际温度值，首先要根据K-1时刻的温度值预测K时刻的温度，按照之前我们讨论的12TT，若k-1时刻的温度值是CTk0123，那么预测此时的CTTkk0123，假如该值的噪声是Ckw05)(，5°是这样得到的，若果k-1时刻估算出的最优温度值的噪声是Ckw0'3)(，预测的噪声是Ckv0'4)(，所以总体的噪声为Ckvkwkw02'2'5)()()(。此时再从温度计上得到K时刻的温度值为CTokz25，设该测量值的噪声是C04。现在发现问题了，在k时刻我们就有了两个温度值CTk023和CTokz25，要信那个呢，简单的求平均已经不能满足精度的要求了。我们可以用他们的协方差covariance来判断。协方差本身就能体现两个信号的相关性，通过它就能判断到底真值更逼近于预测值还是测量值。引入kalmangain（gk），有公式计算gk，)45/(5))()(/()(222'2222kvkwkwkg……（1）所以gk=0.78。我们可以估算出K时刻的实际温度值是，CTTkTTokkzgk56.24)2325(78.023)(……（2）可以看出这个值接近于温度计测量到的值，所以估算出的最优温度值偏向温度计的值。这时我们已经得到了K时刻的最优温度值，接下来估计K+1时刻的最优温度值。既然kalmanfilter是一个最优化的递归处理方法，那么递归就体现在该算法的一个核心参数gk上，由公式（1）gk的算法可知每次计算时的gk是不一样的。这样我们要估计K+1时刻的最优温度值，就得先算出K时刻的gk，然后才能利用公式（2）估计K+1时刻的最优温度值。由此可以看出我们只需知道初始时刻的值和它所对应的协方差以及测量值，就可以进行kalman估计了。3KalmanFilterAlgorithm首先以一个离散控制过程为例讨论kalmanfilteralgorithm。该系统可用一个线性微分方程来描述。)()()1()(kWkUBkXAkX……（3）3)()()(kVkXHkZ……（4）（3）式和（4）式中，)(kX是K时刻的系统状态，)(kU是K时刻对系统的控制量，A和B是系统参数，对于多模型系统，它们为矩阵。)(kZ是K时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。)(kW和)(kV分别表示系统和测量过程中的噪声，使用kalmanfilter估计时，我们认为噪声满足高斯白噪声模型，设)(kW和)(kV的covariance分别为Q和R。讨论kalmanfilteralgorithm的5个经典核心公式。第一步，预测现在的状态：)()1|1()1|(kUBkkXAkkX……（5）式（5）中)1|(kkX是利用上一状态预测的结果，)1|1(kkX是上一时刻的最优预测值，)(kU为现在状态的控制量，如果没有，可以为0。经过公式（5）后系统结果已经更新了，对应于)1|(kkX的covariance还没有更新，用P表示covariance，QAkkPAkkPT)1|1()1|(……（6）式（6）中)1|(kkP是)1|(kkX对应的covariance，)1|1(kkP是)1|1(kkX对应的covariance，TA是A的转置矩阵。Q是系统的噪声，（5）和（6）式便是kalmanfilter中的前两个公式。对系统的预测。有了系统的预测，接下来就要参考测量值进行估计了。))1|()(()()1|()|(kkXHkZkkkkXkkXg……（7）由上面分析可知为了实现递归，每次的gk都是实时更新的。))1|(/()1|()(RHkkPHHKkPkkTTg……（8）)1|())(1()|(kkPHkkkkPg……（9）这样每次)|(kkP和)(kkg都需要前一时刻的值来更新，递归的估计下去。（5）~（9）式便是kalmanfilteralgorithm的五条核心公式。4利用C语言编程实现KalmanFilterAlgorithm4要求是给定一个固定量，然后由测量值来使用kalmanfilter估计系统真实值。为了编程简单，我将（5）式中的A=1，)(kU=0，（5）式改写为下面的形式，)1|1()1|(kkXkkX……（10）式（6）改写为，QkkPkkP)1|1()1|(……（11）再令H=1，式（7），（8），（9）可改写为，))1|()(()()1|()|(kkXkZkkkkXkkXg……（12）))1|(/()1|()(RkkPKkPkkg……（13）)1|())(1()|(kkPkkkkPg……（14）使用C语言编程实现（核心算法）。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为kalmanfilter，R为噪声z_measure=z_real+frand()*0.03;//测量值x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值p_now=(1-kg)*p_mid;//最优值对应的covariancep_last=p_now;//更新covariance值x_last=x_now;//更新系统状态值5算法测试为了测试kalmanfilteralgorithm，我设计了一个简单实验，来验证kalmanfilter的优良性。程序中给定一个初值，然后给定一组测量值，验证kalmanfilter估值的准确性。根据kalmanfilteralgorithm，我们需要给定系统初始值x_last，系统噪声Q和测量噪声R，以及初始值所对应的协方差P_last。为了验证优劣性，还需要给定真实值z_real来计算kalmanfilter误差error_kalman以及测量误差error_measure以及它们在有限次的计算中的累积误差，累积kalman误差sumerror_kalman和累积测量误差sumerror_measure。实验中给定x_last=0，p_last=0,Q=0.018,R=0.0542.实验中可以通过适当改变Q和R来获得更好的估计结果。也可以改变p_last和x_last的值，由于kalmanfilter是对协方差的递归算法来估计信号数据的，所以p_last对算法结果的影响很大，图3就说明了这一情况，由于在初始时就有协方差，所以在运行过程中算法累积误差相比初始时没有误差的就比较大。给定值为z_real=0.56时运行结果如图1所示：5图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对估计结果影响较大，分析可知这种现象是符合kalmanfilter的，通过改变Q和R的值也能改善算法的性能，但是实际操作中我们并不能控制这两个量。6图3改变p_last的测试结果6结论本文通过对kalmanfilteralgorithm的深入探讨，对kalmanfilter有了更深刻的认识，理解了核心的5条公式的物理意义，以及kalmanfilter的思想，并通过理解算法编程实践，验证了kalmanfilter在数据处理方面的优良性能。并且通过实验结果分析了kalmanfilteralgorithm的本质对每次估计产生的协方差递归结合当前测量值来估计系统当前的最佳状态。如要改善算法的性能就必须要尽可能的减小系统噪声和测量噪声，优化程序，减小估计的协方差。参考文献[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.7附录：源程序#includestdio.h#includestdlib.h#includemath.hdoublefrand(){return2*((rand()/(double)RAND_MAX)-0.5);//随机噪声}voidmain(){floatx_last=0;floatp_last=0.02;floatQ=0.018;floatR=0.542;floatkg;floatx_mid;floatx_now;floatp_mid;floatp_now;floatz_real=0.56;//0.56floatz_measure;floatsumerror_kalman=0;floatsumerror_measure=0;inti;x_last=z_real+frand()*0.03;x_mid=x_last;for(i=0;i20;i++){x_mid=x_last;//x_last=x(k-1|k-1),x_mid=x(k|k-1)p_