SLAM 07.后端-卡尔曼滤波

 卡尔曼滤波本质上是一个数据融合算法，将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据融合在一起，得到一个更精确的目的测量值。
 卡尔曼滤波的局限性在于其只能拟合线性高斯系统，但其最大的优点在于计算量小，能够利用前一时刻的状态（和可能的测量值）来得到当前时刻下的状态的最优估计。

1、应用场合

1、卡尔曼滤波器应用在一些无法直接测量只能间接测量的场合。
2、从一些受误差影响的数据中得到系统状态。

2、两个方程式

运动方程式观测方程。

上图中第一行是运动方程式，第二行是观测方程式。运动方程式中由k-1时刻预测到k时刻的状态。
运动方程式中有状态x^k-1，输入u_k，过程误差w^k(例如风速影响）。
观测方程式中有x^k，观测误差v^k（例如GPS的误差）。

3、状态观测器方程模型

假设我们想知道火箭发动机内部的温度，但是内部温度过高，那么只能通过测量外部温度来间接得到内部温度。
知道燃料的输入W^fuel，知道外部测量输出T_ext,我们想知道发动机内部温度Tⁱⁿ。这个时候我们建立一个数学模型，把上面的输入W^fuel接入，得到输出估计值$\hat{T}$_ext。然后让测量值T_ext和估计值$\hat{T}$_ext之差最小，通过一个增益K反馈到预估模型的输入中，这样就形成了一个模拟反馈系统。如果能得到$\hat{T}$_in，则我们认为这就是近似发动机内部的温度。
如果预估值$\hat{T}$_ext比测量值T_ext越大，则调整反馈让输出的下一个预估值越小。选择上述K的方法就是卡尔曼滤波。
把上面的参数更改为如下：
同时我们认为这个模型是线性高斯模型，系统统线性化后如下：

然后进行公式的转换：
上面的解一个指数函数。如果A-KC项小于零，则错误e_abs会随着时间越来越降小，最后 x ˆ \^{x}