卡尔曼滤波算法是一种在 理论模型 和 实际测量 都存在误差时，通过综合理论上的预测值和实际的观测值来获得对当下更准确估计 的方法。

误差

在卡尔曼滤波中存在两种误差：

因为理论模型不够精确导致的 模型误差 。
因为测量手段不够精确导致的 测量误差 。

卡尔曼滤波的主要思想，就是根据模型误差和测量误差的强度，将预测值和观测值融合，从而尽可能地减小误差。

卡尔曼滤波

卡尔曼滤波假设信号满足下面的形式：

$\begin{aligned} x_{k} = F_{k} x_{k - 1} + B_{k} u_{k} + w_{k} \\ z_{k} = H_{k} x_{k} + v_{k} \end{aligned}$

其中 $w_{k}$ 和 $v_{k}$ 是噪声， $x_{k}$ 是信号的真实值， $z_{k}$ 是信号的观测值。由于不可能得知 $x_{k}$ 的精确值，我们用 ${\hat{x}}_{k}$ 来表示 $k$ 时刻对 $x$ 的估计值，用 $P_{k}$ 来表示估计误差 $e_{k} = x_{k} - {\hat{x}}_{k}$ 的协方差矩阵。此外，为了描述观测噪声的强度，我们用 $R_{k}$ 来表示 $v_{k}$ 的协方差矩阵。

在一个新的时间 $k$ 上，我们知道的就是观测值 $z_{k}$ 和上一个时刻的估计值 ${\hat{x}}_{k - 1}$ 。把上一个时刻代入公式，就得到了一个包含理论误差和策略误差的估计值，暂且记作 ${\hat{x}}_{k}^{-}$ :

${\hat{x}}_{k}^{-} = F_{k} {\hat{x}}_{k - 1} + B_{k} u_{k}$

同样的，我们把这个先验估计的误差记为 $e_{k}^{-}$ ，记它的协方差为 $P_{k}^{-}$ ，那么有 $P_{k}^{-} = F_{k} P_{k - 1} F_{k}^{T} + Q_{k}$ 。

把 ${\hat{x}}_{k}^{-}$ 代入观测值的公式，可以得到： $z_{k}^{-} = H_{k} {\hat{x}}_{k}^{-}$ 。实际的观测值是 $z_{k}$ ，我们称它俩的差为 创新(innovation) ，因为这个差值可以提示我们怎么调整 ${\hat{x}}_{k}^{-}$ 。我们把创新记作 ${\tilde{y}}_{k}$ ：

${\tilde{y}}_{k} = z_{k} - H_{k} {\hat{x}}_{k}^{-}$

可以看出，创新来源于 理论误差 和 测量误差 。现在我们希望从创新中分解出理论误差，并用这个误差来更新 ${\hat{x}}_{k}^{-}$ ，即：

${\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H_{k} {\hat{x}}_{k}^{-})$

这个 $K_{k}$ 就是从创新中分解出理论误差的算子。接下来就考虑如何算出这个 $K_{k}$ 。一个重要的结论是：当 $C o v (e_{k}, {\tilde{y}}_{k}) \neq 0$ 时，说明 ${\tilde{y}}_{k}$ 的某种线性组合可以用来使 $e_{k}$ 减小。因此 最优线性估计的误差与用于矫正的信息不相关 ，即 $C o v (e_{k}, {\tilde{y}}_{k}) = 0$ 。
那么：

$\begin{aligned} C o v (e_{k}, {\tilde{y}}_{k}) \\ = & C o v (x_{k} - {\hat{x}}_{k}, z_{k} - H_{k} {\hat{x}}_{k}^{-}) \\ = & C o v (x_{k} - {\hat{x}}_{k}^{-} - K_{k} (z_{k} - H_{k} {\hat{x}}_{k}^{-}), z_{k} - H_{k} {\hat{x}}_{k}^{-}) \\ = & C o v (e_{k}^{-}, z_{k} - H_{k} {\hat{x}}_{k}^{-}) - K_{k} C o v (z_{k} - H_{k} {\hat{x}}_{k}^{-}, z_{k} - H_{k} {\hat{x}}_{k}^{-}) \\ = & 0 \end{aligned}$

又 $z_{k} - H_{k} x_{k}^{-} = H_{k} (x_{k} - {\hat{x}}_{k}^{-}) + v_{k} = H_{k} e_{k}^{-} + v_{k}$ ，因此 $C o v (e_{k}^{-}, z_{k} - H_{k} {\hat{x}}_{k}^{-}) = P_{k}^{-} H^{T}$ ，同理 $C o v (z_{k} - H_{k} {\hat{x}}_{k}^{-}, z_{k} - H_{k} {\hat{x}}_{k}^{-}) = H P_{k}^{-} H^{T} + R_{k}$ 。则：

$P_{k}^{-} H^{T} = K_{k} (H P_{k}^{-} H^{T} + R_{k})$

所以

$K_{k} = P_{k}^{-} H^{T} (H P_{k}^{-} H^{T} + R_{k})^{- 1}$

然后可以得到:

$\begin{aligned} P_{k} \\ = & C o v (e_{k}) \\ = & C o v (x_{k} - {\hat{x}}_{k}) \\ = & C o v (e_{k}^{-} - K_{k} (z_{k} - H_{k} {\hat{x}}_{k}^{-})) \\ = & C o v (e_{k}^{-} - K_{k} (H_{k} x_{k} + v_{k} - H_{k} {\hat{x}}_{k}^{-})) \\ = & C o v ((I - K_{k} H_{k}) e_{k}^{-} - K_{k} v_{k}) \\ = & (I - K_{k} H_{k}) P_{k}^{-} (I - K_{k} H_{k})^{T} + K_{k} R_{k} K_{k}^{T} \end{aligned}$

扩展卡尔曼滤波算法(EKF)

在上面的卡尔曼滤波算法中，我们只考虑了线性模型下的情况。但真实世界往往是非线性的，这就引入了扩展卡尔曼滤波。扩展卡尔曼滤波的核心是用非线性函数的一阶雅可比矩阵来将函数近似为一个线性函数。

假设非线性模型如下:

$\begin{aligned} x_{k} & = f (x_{k - 1}, u_{k}) + w_{k} \\ z_{k} & = h (x_{k}) + v_{k} \end{aligned}$

扩展卡尔曼滤波的过程如下：

计算 $F_{k}$ : $F_{k} = {\frac{\partial f}{\partial x} |}_{{\hat{x}}_{k - 1}, u_{k}}$ 。
传播先验协方差 : $P_{k}^{-} = F_{k} P_{k - 1} F_{k}^{T} + Q_{k}$
用模型来计算先验估计 : ${\hat{x}}_{k}^{-} = f ({\hat{x}}_{k - 1}, u_{k})$
计算 $H_{k}$ : $H_{k} = {\frac{\partial h}{\partial x} |}_{{\hat{x}}_{k}}$
计算卡尔曼增益： $K_{k} = P_{k}^{-} H^{T} (H P_{k}^{-} H^{T} + R_{k})^{- 1}$
计算创新 : ${\tilde{y}}_{k} = z_{k} - h ({\hat{x}}_{k}^{-})$
计算后验估计： ${\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} {\tilde{y}}_{k}$ 。
更新 $P_{k}$ : $P_{k} = (I - K_{k} H_{k}) P_{k}^{-} (I - K_{k} H_{k})^{T} + K_{k} R_{k} K_{k}^{T}$