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

卡尔曼滤波算法及 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 条公 式的物理意义。 假设我们要研究的对象是某一个房间的温度信号。 对于室温来说， 一分钟内或一小段时 间内的值是基本上不变的或者变化范围很小。也就是说 t 时刻的温度 T 和 t 时刻的温度 T 1 1 2 2 基本不变，即 T T 。在这个过程中，因为毕竟温度还是有所改变的，设有几度的偏差。 2 1 我们把这几度的偏差看成是高斯白噪声 w (t ) ，也就是说 E[ w(t )] 0 ， D[ w(t )] 2 。除此 之外我们在用一个温度计来实时测量房间的温度值 Z ，但由于量具本身的误差，所测得的 温度值也是不准确的， 也会和实际值偏差几度， 把这几度的偏差看成是测量噪声 v (t ) 。即满 足 E[v(t )] 0 ， D[v(t )] 2 。 1 此时我们对于这个房间的温度就得到了两个数据。一个是你根据经验得到的经验值 T T ，一个是从温度计上得到的测量值 Z ，以及各自引入的高斯白噪声。下面就具体讲 2 1 1 解 kalman filter 来估计房间温度的原理与步骤。 要估计 K 时刻的实际温度值， 首先要根据 K-1 时刻的温度值预测 K 时刻的温度， 按照之