卡尔曼滤波算法语言实现.docVIP

PAGE 1 卡尔曼滤波算法及C语言实现 摘要：本文着重讨论了卡尔曼滤波器的原理，典型算法以及应用领域。清晰地阐述了kalman filter在信息估计方面的最优性能。着重介绍简单kalman filter algorithm的编程，使用kalman filter的经典5个体现最优化递归公式来编程。通过c语言编写程序实现kalman filter的最优估计能力。 关键词：kalman filter；最优估计；C语言 1 引言 Kalman Filter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。起源于Rudolf Emil Kalman在1960年的博士论文和发表的论文《A New Approach to Linear Eiltering and Prediction Problems》（《线性滤波与预测问题的新方法》）。并且最先在阿波罗登月计划轨迹预测上应用成功，此后kalman filter取得重大发展和完善。它的广泛应用已经超过30年，包括机器人导航，控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，近年来更被广泛应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2 kalman filter最优化递归估计 Kalman filter是一个“optimal recursive data processing algorithm（最优化递归数据处理方法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的方法。而kalman filter最为核心的内容是体现它最优化估计和递归特点的5条公式。举一个例子来详细说明5条公式的物理意义。 假设我们要研究的对象是某一个房间的温度信号。对于室温来说，一分钟内或一小段时间内的值是基本上不变的或者变化范围很小。也就是说时刻的温度和时刻的温度基本不变，即。在这个过程中，因为毕竟温度还是有所改变的，设有几度的偏差。我们把这几度的偏差看成是高斯白噪声，也就是说，。除此之外我们在用一个温度计来实时测量房间的温度值，但由于量具本身的误差，所测得的温度值也是不准确的，也会和实际值偏差几度，把这几度的偏差看成是测量噪声。即满足，。 此时我们对于这个房间的温度就得到了两个数据。一个是你根据经验得到的经验值，一个是从温度计上得到的测量值，以及各自引入的高斯白噪声。下面就具体讲解kalman filter来估计房间温度的原理与步骤。 要估计K时刻的实际温度值，首先要根据K-1时刻的温度值预测K时刻的温度，按照之前我们讨论的，若k-1时刻的温度值是，那么预测此时的，假如该值的噪声是，5°是这样得到的，若果k-1时刻估算出的最优温度值的噪声是，预测的噪声是，所以总体的噪声为。此时再从温度计上得到K时刻的温度值为，设该测量值的噪声是。 现在发现问题了，在k时刻我们就有了两个温度值和，要信那个呢，简单的求平均已经不能满足精度的要求了。我们可以用他们的协方差covariance来判断。协方差本身就能体现两个信号的相关性，通过它就能判断到底真值更逼近于预测值还是测量值。引入kalman gain（），有公式计算， ……（1） 所以=0.78。我们可以估算出K时刻的实际温度值是， ……（2） 可以看出这个值接近于温度计测量到的值，所以估算出的最优温度值偏向温度计的值。 这时我们已经得到了K时刻的最优温度值，接下来估计K+1时刻的最优温度值。既然kalman filter是一个最优化的递归处理方法，那么递归就体现在该算法的一个核心参数上，由公式（1）的算法可知每次计算时的是不一样的。这样我们要估计K+1时刻的最优温度值，就得先算出K时刻的，然后才能利用公式（2）估计K+1时刻的最优温度值。由此可以看出我们只需知道初始时刻的值和它所对应的协方差以及测量值，就可以进行kalman估计了。 3 Kalman Filter Algorithm 首先以一个离散控制过程为例讨论kalman filter algorithm。该系统可用一个线性微分方程来描述。 ……（3） ……（4） （3）式和（4）式中，是K时刻的系统状态，是K时刻对系统的控制量，A和B是系统参数，对于多模型系统，它们为矩阵。是K时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。和分别表示系统和测量过程中的噪声，使用kalman filter估计时，我们认为噪声满足高斯白噪声模型，设和的covariance分别为Q和R。 讨论kalman filter algorithm的5个经典核心公式。 第一步，预测现在的状态： ……（5） 式（5）中是利用上一状态预测的结果，是上一时刻的最优预测值，为现在状态的控制量，如果没有，可以为0。 经过公式（5）后系