卡尔曼滤波（四）（最优化数字处理算法）

一、先验误差的协方差矩阵求解

---

引入过程噪音，计算得到的状态变量：X（k）= A2 *  X（k-1）+ B2 * U（k）+ W（k-1）

引入测量噪音后，测量得到的状态变量：Z（k）= H * X（k）+ V（k）

对于X（k）的理解：

（1）X（k）是引入了不确定性后得到的状态变量，它弥补了由于模型不够准确而缺失的那部分影

响，它是我们想要的真实的结果，但是由于W（k-1）这部分无法得到，所以X（k）得不到

（2）如果在现实得到的Z（k）中剔除了V（k）成分，就可以得到X（k），它是我们想要的真实的

结果，但是由于V（k）这部分无法得到，所以X（k）得不到

---

先验估计值：X_Hat（k）= A2 * X_Hat_New（k-1）+ B2 * U（k）

先验误差：E（k）= X（k）-X_Hat（k) 

过程噪音的协方差矩阵：Q = E【W * W的转置】

先验误差可用它的协方差矩阵来量化表示：P【E（k）】 = E【E（k）* E（k）的转置】

卡尔曼增益系数：K（k） = （ H * P【K（k）】* H的转置 + R ）的逆 * P【K（k）】* H的转置

---

这一章是为了求解先验误差的协方差矩阵P【K（k）】的表达式

---

而先验误差E（k） = X（k）-X_Hat（k)  ，X（k）和X_Hat（k）的表达式都知道，最后可以得到

先验误差E（k）的表达式：E（k） = A2 * E（k-1）+ W（k-1），然后把这个表达式代入到先验误

差的协方差矩阵的表达式当中，也就是P【E（k）】 = E【E（k）* E（k）的转置】，最后可得：

P【E（k）】= A2 * P_New【 E_New（k-1）】* A2的转置 + Q

二、使用卡尔曼滤波器的步骤

---

主要分为三个步骤：

（1）第一步是建立估计方程和测量方程（注意：由于传感器得到的值，已经包括测量噪音，所以

测量方程可以不用再加入测量噪音这一项；而估计方程在建立的时候，可以把外部的干扰引入到状

态变量这个向量里面，也可以不引入）

（2）第二步是预测

（3）第二步是校正

也就是使用上一次的最优结果（也就是后验估计值）预测这一次的值（也就是先验估计值），同时

使用测量值来修正这一次的预测值，来得到这一次的最优结果

---

对于预测，先求解先验估计值，再求解先验误差的协方差矩阵

先验估计值：X_Hat（k）= A2 * X_Hat_New（k-1）+ B2 * U（k）

先验误差的协方差矩阵：P【E（k）】= A2 * P_New【 E_New（k-1）】* A2的转置 + Q

---

对于校正，先求解卡尔曼增益系数，再求解后验估计值给下一次计算先验误差值做准备，最后更新

后验误差的协方差矩阵给下一次计算先验误差的协方差矩阵做准备

卡尔曼增益系数：K（k） = （ H * P【E（k）】* H的转置 + R ）的逆 * P【E（k）】* H的转置

后验估计值：X_Hat_New（k) = X_Hat（k) + K（k）*【Z（k) - H * X_Hat（k）】

更新后验误差的协方差矩阵：P_New【 E_New（k）】=（单位阵I - K（k）* H）* P【E（k）】

（这个表达式在第三篇中末尾处有给出P_New【 E_New（k）】的由来，再把K（k）代入即可）

最后发现，卡尔曼增益系数是跟Q矩阵和R矩阵有关的，对应于过程噪音和测量噪音的协方差矩阵

三、初始化参数的设置

---

调节Q矩阵和R矩阵，Q矩阵取决于模型建立与现实的差距，R矩阵取决于传感器测量的精度和现实

的干扰

设置第一次后验估计值和后验误差的协方差矩阵，这里可以随意设置，经过几次迭代计算之后就可

以趋于理论稳定值。习惯设置后验估计值都为0，后验误差的协方差矩阵往小的进行取值，一般为1

但不可以为0