【多传感器融合】卡尔曼滤波

卡尔曼滤波

在这一部分，就来描述源于 Dr.Kalman 的卡尔曼滤波器。接下来对卡尔曼滤波过程进行说明。当前状态为 k ，根据先前状态最优结果预测其当前状态：

当前系统结果已经更新，但X（- k） 的协方差还没更新，用 P 表示协方差：

以上部分就是卡尔曼滤波器 5 个公式当中的前两个，也就是系统的预测部分。有了现在状态的预测结果后，还需要现在状态的测量值。通过预测值和测量值，可以得到当前状态 k 时的最优化估算值：

卡尔曼增益Kk 由下式计算得到：

得到状态 k 时的最佳估计后，为了使 Kalman 滤波器继续运行直到系统进程结束，需要更新 k 状态下的协方差：

当系统进入 k +1状态时，Pk 就是第二个式子的P（- K），算法就可以自回归的运算下去。
系统工作具体流程如图：