卡尔曼滤波project

基于卡尔曼滤波的随机性运动跟踪设计方法 1、实验目的： 本实验通过运用 Kalman(卡尔曼)滤波来预测做随机性运动的物体 的位置，利用基于 opencv2.4 的 vs2010 编程环境实现跟踪仿真。 2、实验原理： 1. 卡尔曼滤波： Kalman 滤波器是信号处理领域的重要方法，近年来更被运用到 图像处理领域，例如人脸识别、运动跟踪、图像分割、图像边缘检 测等。简单的说，卡尔曼滤波是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法） ”。 Kalman 滤波器的基本思想是，若有一组强而合理的假设，给出 系统的历史测量值，则可以建立最大化这些早前测量值的后验概率 的系统状态模型。Kalman 滤波需要三个重要的假设：（ 1）被建模 的系统是线性的；（2）影响测量的噪声属于白噪声；（3）噪声本 质上是高斯分布的。第一条假设表明 k 时刻的系统状态可以用某个 矩阵与 k-1 时刻的系统状态的乘积表示。另外两条假设，即假设噪 声是高斯分布的白噪声，其含义为噪声与时间不相关，且只用均值 和协方差就可以准确地为幅值建模。 2. 卡尔曼滤波器算法 用一个线性随机微分方程描述一个离散控制过程的系统： x(k)= F x(k-1) + B u(k) + w(k) x(k)为一个状态元素的 n 维向量，F 称为状态转移矩阵，u(k)表示 k 时刻对系统的控制量，B 是一个联系输入控制盒状态改变的矩阵， w(k)为过程噪声向量，被假设成高斯白噪声。 系统测量值： z(k)=H x(k)+v(k) H 为测量系统参数，v(k)是观测噪声向量，是在观测过程中外界对 观测值的噪声干扰，也被假设成高斯白噪声。首先我们要利用系统 的过程模型，来预测下一状态的系统。假设现在的系统状态是 k， 根据系统的模型，可以基于系统的上一状态而预测出现在状态： x(k|k-1)=F x(k-1|k-1)+B u(k) (1) 式(1)中，x(k|k-1)是利用上一状态预测的结果，x(k-1|k-1)是上一 状态最优的结果，u(k)为现在状态的控制量，如果没有控制量，它 可以为 0。 基于式(1),系统结果已经更新了，可是，对应于 x(k|k-1)的 covariance 还没更新。我们用 p 表示 covariance： p(k|k-1)=F p(k-1|k-1) FT+Q (2) 式 (2)中， p(k|k-1)是 x(k|k-1)对应的 covariance，F T表示 F 的转 置矩阵，Q 是系统过程的 covariance。式(1)、(2)为对系统的预测。 结合预测值和测量值，我们可以得到现在状态(k)的最优化估算 值 x(k|k)：x(k|k)= x(k|k-1)+Kg(k) (z(k)-H x(k|k-1) (3) 陈检根 12212882 模式识别与智能系统 其中 Kg 为卡尔曼增益(Kalman Gain)： Kg(k)= p(k|k-1) HT/(H p(k|k-1) HT+ R) (4) 据此，可得到 k 状态下最优的估算值 x(k|k)。为使卡尔曼滤波 器不断的运行下去直到系统过程结束，须更新 k 状态下 x(k|k)的 covariance： p(k|k)=（I-Kg(k) H）p(k|k-1) (5) 其中 I 为 1 的矩阵(单位矩阵)，对于单模型单测量，I=1。当系统 进入 k+1 状态时，p(k|k)就是式子(2)的 p(k-1|k-1)。这样，算法 就可以自回归的运算下去。 3. 随机性运动跟踪的状态分析： Kalman 滤波器应用中的随机运动被假设成高斯模型，在融合一 个新的测量值前先进行“更新”以便将动态变化包含进仿真模型中。 具体实例比如测量在停车场行驶的汽车运动状态，包括两个位置变 量 x 和 y，以及两个速度变量 vx和 vy，则状态变量为： x(k)=x y vx vyt,F= 01dt 使用摄像头去测量汽车的状态时，可能只测量到位置变量： z(k)=zx zyt 系统参数 H 结构类似于下面所示： H= 01 在该实例中，认为汽车速度是不变的，所以要设置一个 Qk反映这个 问题。先计算状态的先验估计 x-(k)如下： x-(k)=Fx(k-1)+Bu(k-1)+w(k) 用 P-(k)表示误差协方差，为：P -(k)=FP(k-1)FT+Q(k-1), Kalman 更新率为 K(k)=P-(k)HT(HP-(k)HT+R)-1,那么在获得新的测量 值时，可以计算 x(k)和 P(k)最优的更新值： x(k)=x-(k)+K(k)(z-(k)-Hx-(k) P(k)=(I-K(k)H)P-(k) 根据 Kalman 滤波器的递推计算可以计算出汽车的预估值。 4. OpenCV 和 Kalman 滤波器： OpenCV 提供了四个与应用 Kalman 滤波器直接相关的函数： （1）cvCreatKalman()产生和返回一个 cvKalman 数据结构的指针； （2）cvReleaseKalman()则是删除一个 cvKalman 数据结构； （3）cvKalmanPredict()在 cvKalman 结构被赋值后，被调用来计算 下一个时间点的预期值； （4）cvKalmanCorrect()校正新的测量值。后两个函数运行完后， 可获得被跟踪系统的状态。 5. 程序实现及程序流程图： 本程序通过使用 opencv 的图形库，经过创建和初始化 kalman 滤 波的参数，再调用相关 kalman 滤波函数，最终得出物体运动轨迹图。 陈检根 12212882 模式识别与智能系统 程序流程图如下： N Y 三、实验步骤： 1. 搭建实验环境： 开始 创建窗口 创建并初始化随机 创建并初始化 xk、F、wk、vk、H 程序结束 计算 xk、zk、yk 初始化其他 Kalman 参数 绘出物体运动轨迹 是否按下 ESC 键 本实验是基于 opencv2.4+vs2010 的运行环境，首先需要安装 vs2010 以及 opencv2.4。然后将 opencv2.4 图形库配置到 vs2010 中， 配置过程包括操作系统环境变量的配置、vs 项目环境中的配置 （opencv 的包含目录文件路径的添加以及依赖库的添加）等，具体 步骤不再赘述。 2. 新建 win32 控制台项目，将实现代码的源程序添加到项目中， 并保存项目。 3. 开始调试，直到生成成功，期间通过调试不断改正程序。 4. 点击执行，显示程序执行结果。 5. 观察分析实验结果，并做出相应的记录，分析实验结果与预 期实验要求的异同。 6. 退出程序，实验结束。 4、实验结果： 程序执行后生成两个显示框，一个显示框显示的是实验物体在不 同时刻 x 和 y 方向的速度，另一个显示框则是物体运动轨迹图，其 中白色球是目标物体，即物体运动的真实轨迹，实验中是设定为随 机性运动，蓝色球是观测球，绿色球是 Kalman 估计球，它表明 Kalman 滤波对物体运动轨迹的预估值。 实验中发现，随着物体在四壁上来回撞击，物体的速度在不断的 变化。Kalman 滤波通过观测球和目标球，预估出物体的下一个位置， 当白色球撞壁反弹时，绿球被“甩”出弹壁，这个现象是因为绿球 陈检根 12212882 模式识别与智能系统 以之前的状态，估计白球还会往前走，但被甩出去后，它根据自己 的模型和实际观测值重新更新模型，从而使自己重新较准确地跟踪 白球。 实验结果图示如下： 图 1：物体 x 方向（左列）和 y 方向（右列）速度变化 图 2：物体运动轨迹变化图 5、实验感受和心得： 通过对卡尔曼滤波及其算法的学习和运用，比较深刻的体会到卡 尔曼滤波基于状态空间模型的线性最优效果。从最初的确定实验目 的到后期的程序实现，期间除了更加了解 vc+的编程环境，对 opencv 图形库的深层次了解和运用也是这次项目的一大收获。在编 写和调试程序的过程中，除了要经常翻阅学习 opencv这本书之 外，还需