Introduction
卡尔曼滤波，用直白的话来讲，就是利用观测值修正预测值，以得到最优的估计值。
- 先举个具象的例子，估计直流电机的转速。
- 预测值：我们对直流电机会建立一个模型，通过上一时刻的转速和输入电压，可以预测这一时刻的转速。显然，人为建立的模型和物理世界中实际电机的模型会有误差的，因此预测值不可全信。
- 观测值：我们可以通过外在的传感器来测量电机的转速。假如传感器可以测量真实值，我们可以直接选用测量值；但实际上，传感器测量也是有误差的，因此测量值不可全信。
- 估计值：预测值和观测值是我们能够获取的对于电机转速的信息，我们能做的就是将这两个信息相结合（用观测值修正预测值），以得到最优的估计值，至于怎么最优，后续原理推导中讲解。
Preliminary
学过现代控制理论的都知道，怎么建立一个状态空间模型：
\[\begin{align} \dot{x} =& A x + B u \\ y =& C x + D u \end{align}\]然后，考虑一下卡尔曼滤波的应用场景，需要离散化一下，同时假设观测与控制输入无关
\[\begin{align} x_k =& F_k x_{k-1} + B_k u_k \\ z_k =& H_k x_k \end{align}\]上述模型是理想条件下的，因为实际过程中会有噪声，因此实际模型如下
\[\begin{align} x_k =& F_k x_{k-1} + B_k u_k + w_k \\ z_k =& H_k x_k + v_k \end{align}\]其中，假设过程噪声为高斯噪声，$w_k \sim N(0, Q_k)$，$v_k \sim N(0, R_k)$
原理推导
原理推导将围绕卡尔曼滤波的几个关键方程和概念（具体名字不同的文章可能不同）展开。首先，是状态量相关的。
-
状态方程： $ x_k = F_k x_{k-1} + B_k u_k + w_k $
-
状态预测方程： $ \hat{x_{k|k-1}} = F_k \hat{x}_{k-1} + B_k u_k $
状态方程指的是模型在实际情况下的方程，而状态预测方程指的是模型在理想情况下的方程。因为我们不知道每一时刻的噪声，因此只能用状态预测方程去近似状态方程。这里几个符号说明一下：$x_k,x_{k-1}$ 指的是真实值，$\hat{x_{k|k-1}}$ 指的是基于上一刻的某一个值，预测的当前时刻的状态值。 $\hat{x}_{k-1}$ 可以是上一时刻的预测值，可以是上一时刻的估计值（基于观测优化的估计值，如果知道更好），可以是上一时刻的真实值（如果知道是最好，一般不知道）。所以，状态预测方程就是基于上一刻的某一个值（预测值、估计值、真实值），预测当前时刻的状态值（预测值）。
假设：不知道真实状态值，但每一时刻都有观测值，则状态预测方程中 $\hat{x}_{k-1}$ 指的是上一时刻的估计值。
-
状态预测误差：$\widetilde{x_{k|k-1}} = x_k - \hat{x_{k|k-1}}$
状态预测方程终究不是实际的状态方程，因此状态预测肯定会有误差。这里，状态预测误差可以进一步展开：
\[\begin{align} \widetilde{x}_{k|k-1} &= x_k - \hat{x}_{k|k-1} \\ &= (F_k x_{k-1} + B_k u_k + w_k) - (F_k \hat{x}_{k-1} + B_k u_k) \\ &= F_k(x_{k-1} - \hat{x}_{k-1}) + w_k \end{align}\]然后，来理解一下状态预测误差。状态预测误差是怎么来的？看看状态方程和状态预测方程（假设知道初始时刻的真实值），很明显误差来自于噪声 $w_k$，由于我们假设了噪声是高斯分布，是一种概率分布，那是不是可以推导出，状态预测误差也是一种概率分布、一种高斯分布（每一时刻累积，高斯分布+高斯分布=高斯分布）。且因为噪声高斯分布是零均值的，所以状态预测误差也是零均值的。
-
状态预测误差 协方差矩阵（方差）：
\[\begin{align} P_{k|k-1} &= cov\{x_k - \hat{x}_{k|k-1}\} \\ &= cov\{F_k(x_{k-1} - \hat{x}_{k-1}) + w_k\} \\ &= F_k P_{k-1|k-1} F_k^T + Q_k \end{align}\]$P_{k|k-1}$ 指的是基于 $k-1$ 时刻估计值预测 $k$ 时刻的预测值和 $k$ 时刻的真实值之间的误差的协方差。
$P_{k-1|k-1}$ 指的是 $k-1$ 时刻的估计值和 $k-1$ 时刻真实值之间的误差的协方差。
这条式子给出了 $P_{k-1|k-1}$ 向 $P_{k|k-1}$ 迭代的过程。
接着，要介绍观测量相关。
-
观测方程：$z_k = H_k x_k + v_k$
-
观测预测方程：$\hat{z_{k|k-1}} = H_k \hat{x_{k|k-1}}$
观测方程指的是实际情况下，对于模型状态量的观测，因此会有噪声，所以这里的 $z_k$ 是你实际观测到的。而观测预测方程，并不是让你去观测，而是让你去预测观测量。这里 $\hat{z_{k|k-1}}$ 指的是，基于上一时刻的状态估计量预测这一时刻的状态量，再基于这一时刻的预测的状态量，预测这一时刻的观测量。
-
观测预测误差：$\widetilde{z_{k|k-1}} = z_k - \hat{z_{k|k-1}}$
这里的观测预测误差和上文的状态预测误差理解是基本一致的。可以看到误差来自于这么几项：观测方程的误差（噪声）$v_k$ 、状态预测误差 $\widetilde{x_{k|k-1}}$。同样这里的两个误差都是呈零均值的高斯分布，因此观测预测误差也是呈零均值的高斯分布。
-
观测预测误差 协方差矩阵（方差）：
\[\begin{align} S_k &= cov\{\widetilde{z}_{k|k-1}\} \\ &= cov\{z_k - \hat{z}_{k|k-1}\} \\ &= cov\{(H_k x_k + v_k)-(H_k \hat{x}_{k|k-1})\} \\ &= cov\{H_k(x_k -\hat{x}_{k|k-1}) + v_k\} \\ &= H_kP_{k|k-1}H_k^T+R_k \end{align}\]$S_k$ 指的是基于 $k-1$ 时刻的状态估计值预测 $k$ 时刻的观测量和 $k$ 时刻真实的观测量之间误差的协方差。
OK，接下来来到卡尔曼滤波的核心，即如何使用观测量来修正状态预测值，以得到状态估计值。
-
修正方程：$\hat{x_{k|k}} = \hat{x_{k|k-1}} + K_k\widetilde{z_{k|k-1}}$
其中，$K_k$ 为 $k$ 时刻的卡尔曼增益，也是卡尔曼滤波要设计的核心参数。
那么设计目标是什么？很显然，希望估计值 $\hat{x_{k|k}}$ 和真实值 $x_k$ 之间的误差尽可能小嘛。又有之前对于误差的理解可以知道，误差是零均值的高斯分布，那么设计目标可以进一步明确为，最小化估计误差的方差。
-
状态估计误差 协方差矩阵（方差）：
\[\begin{align} P_{k|k} &= cov\{x_k - \hat{x}_{k|k}\} \\ &= cov\{x_k - (\hat{x}_{k|k-1} + K_k\widetilde{z}_{k|k-1})\} \\ &= cov\{x_k - \hat{x}_{k|k-1} - K_k(z_k - \hat{z}_{k|k-1})\} \\ &= cov\{x_k - \hat{x}_{k|k-1} - K_k(H_k(x_k -\hat{x}_{k|k-1}) + v_k)\} \\ &= cov\{x_k - \hat{x}_{k|k-1} - K_kH_k(x_k -\hat{x}_{k|k-1}) + K_kv_k\} \\ &= cov\{(I - K_kH_k)(x_k -\hat{x}_{k|k-1}) + K_kv_k\} \\ &= (I - K_kH_k)P_{k|k-1}(I - K_kH_k)^T + K_kR_kK_k^T \end{align}\]而根据协方差的定义，可以将这里的协方差矩阵展开：
\[P_{k|k} = \begin{pmatrix} E(x_{1k} - \tilde{x}_{1k})^2 & E(x_{1k} - \tilde{x}_{1k})(x_{2k} - \tilde{x}_{2k}) & \cdots & \cdots \\ E(x_{2k} - \tilde{x}_{2k})(x_{1k} - \tilde{x}_{1k}) & E(x_{2k} - \tilde{x}_{2k})^2 & \cdots & \cdots \\ \cdots & \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots & E(x_{nk} - \tilde{x}_{nk})^2 \end{pmatrix}\]而我们的目标是最小化估计误差的方差，即最小化 $tr(P_{k|k})$，那么最简单的思路，找到 $tr(P_{k|k})$ 对 $K_k$ 导数为零的点。
\[\frac{\mathrm{d} \text{tr}(P_{k|k})}{\mathrm{d} K_k} = -2(H_k P_{k|k-1})^T + 2K_k S_k = 0\]则可以解得所需的卡尔曼增益
\[K_k = P_{k|k-1} H_k^T S_k^{-1}\]因此，将卡尔曼增益回代，状态估计误差协方差矩阵（方差）又可以化为
\[\begin{aligned} P_{k|k} &= P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} H_k^T K_k^T + K_k S_k K_k^T \\ &= P_{k|k-1} - K_k H_k P_{k|k-1} \end{aligned}\]到此我们已经完成了卡尔曼滤波的所有理论推导。
