基于卡尔曼滤波的策略研究（上）.docxVIP

一、 卡尔曼滤波 （一）卡尔曼滤波的介绍 卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数值，对系统状态进行最优估计的算法。Rudolf Emil Kalman于 1960 年发表论文《A New Approach to Linear Filtering and Prediction Problems 》， 此后卡尔曼滤波正式走入大众视野。卡尔曼滤波较维纳滤波 （Wiener filter）有着里程碑式的突破，维纳滤波只适用于平稳随机过程，且应用时需要得到全部过去观测值，而卡尔曼滤波不受限于这 2 点，因此应用也更广泛。 在介绍算法之前，作者先使用一个简单的体重测量例子做个直观解释，方 便读者理解。假设现在需要估算同学 A 在 k 时刻的实际体重，首先需要根据 k-1 时刻的体重，来预测 k 时刻的体重。因为体重很难在短期发生显著改变，因此 个人认为 k 时刻的体重和 k-1 时刻的体重基本是一样的，假设是 60kg，有±2kg 的误差。然后，在体重称上得出 k 时刻的体重为 61kg，因为体重称不一定准确，所以设定了±1kg 的误差。现在有两个体重值来估算同学 A 在 k 时刻的真实体重，分别为 60kg 和 61kg。依据 2 个值的误差和协方差，我们可以得出卡尔曼增益 K （Kalman Gain），此处 K 为：√ 22 22+12 =0.89,卡尔曼增益后续会详细解释。因此 k 时刻的实际体重的最优值为：60+0.89*（61-60）=60.89，且该最优值的偏差为： √（1 ? 0.89） ? 22=0.66。其中 2 就是 k 时刻个人预测体重时的偏差，得出的 可以被看成是进入 k+1 时刻后在 k 时刻算出的最优值的偏差，该值对应了 k 时刻体重称的误差 1。之后，每次测算体重，都用这个体重称和个人经验去估计体重，这就是卡尔曼滤波的过程。 （二）卡尔曼滤波器算法 根据上文的简单例子可以发现，线性动态系统可以用 2 个线性方程来描述， 这 2 个方程分别为状态方程和观测方程，如下： 状态方程: ???? = ???????1 + ???????1 + ?????1 观测方程: Zk = ?????? + ???? 状态方程的作用是估计出先验的状态，其中????代表了 k 时刻的系统状态， ????代表了 k 时刻对系统的控制量，矩阵 A 和 B 是系统参数矩阵，分别代表了状 态转移矩阵和控制量转移矩阵，如果需要做卡尔曼滤波的对象没有控制量输入， ??????这一项可以被省略。观测方程中Zk代表了 k 时刻的测量值，矩阵 H 是测量系统的参数矩阵。这两个方程中????和????分别代表了估计和测量的噪音，都满足高斯分布，Q 和R 代表了这 2 个噪音的方差。 卡尔曼滤波就是通过综合考虑了由以上 2 个方程得出的估计值和测量值， 迭代出不确定性最小的数值。其实现过程可以细分为 5 步，这 5 步可以用 5 个公式来进行描述，如下： ??1. ???? ?? = ????????1 + ???????1 2. ????? = ???????1???? + ?? 3. ???? = ????????? ???????????+?? ??4. ????? = ???? ?? + ?? （?? ? ?????? ) ??????5. ???? = (?? ? ??????)????? (I 为单位矩阵) ?? ?? ?? 来细看每一个公式，首先是第一个公式，基于 t-1 时刻的状态以及控制量对 t 时刻的状态进行估计。第二个公式是利用 t-1 时刻得出的最优估计值的协方差和估计方差得出 t 时刻公式（1）的先验状态估计的协方差矩阵，单看公式难以理解，因此对公式进行了详细的推导，如下： ?????为先验状态估计的协方差矩阵，所以： ??? = ??????(???? ，???? ) = ??[(???? ? ?? )(???? ?? ? ?? ] ?? ?? ?? ?? ?? ?? ??) 其中（???? ? ?? ）可以被称为先验误差??? ，所以： ?? ?? ?? ???? =（???? ? ????） ??= (????????1 + ???????1 + ??) ? (???????1 + ???????1) ?? = ??(??????1 ? ?????1) + ?? = ???????1 + ?? 所以： ????? = ??(????? ????? ??) = ??[(???????1 + ??)(???????1 + ??)??] ???1= ??[(???????1 +