假设线性系统（满足马尔可夫性质）的状态方程和观察方程为

\[\begin{align} Initial&: bel(x_0)=p(x_0)=\det(2\pi{\Sigma}_t)^{-\frac{1}{2}}\exp\left(-\frac{1}{2}(x_0-\mu_0)^T{\Sigma}_0^{-1}(x_0-\mu_0)\right)\\ x_t &= A_tx_{t-1}+B_tu_t+\varepsilon_t\\ z_t &= C_tx_t+\delta_t\\ \varepsilon_t &\thicksim N(0,R_t)\\ \delta_t &\thicksim N(0,Q_t)\\ \end{align}\]

先给出 KF 算法

\[\begin{align} \textbf{Input}&: \{\mu_{t-1},{\Sigma}_{t-1},u_t,z_t\}\\ \textbf{Output}&: \{\mu_t,{\Sigma}_t\}\\ \bar{\mu}_t &= A_t\mu_{t-1}+B_tu_t\\ \bar{\Sigma}_t &= A_t{\Sigma}_{t-1}A_t^{\top}+R_t\\ K_t &= \bar{\Sigma}_tC_t^{\top}\left(C_t\bar{\Sigma}_tC_t^{\top}+Q_t\right)^{-1}\\ \mu_t &= \bar{\mu}_t+K_t(z_t-C_t\bar{\mu}_t)\\ {\Sigma}_t&=(I-K_tC_t)\bar{\Sigma}_t \end{align}\]

预测阶段

根据全概率公式有

\[\begin{align} \bar{bel}(x_t)&=\int \underbrace{p(x_t|x_{t-1},u_t)}_{\sim N(x_t;A_tx_{t-1}+B_tu_t,R_t)} \quad\underbrace{bel(x_{t-1})}_{\sim N(x_{t-1};\mu_{t-1},\Sigma_{t-1})}dx_{t-1}\\ &=\mu\int \exp\{-L_t\}dx_{t-1} \end{align}\]

式中指数部分

\[\begin{align} L_t &= \frac{1}{2}(x_t-A_tx_{t-1}-B_tu_t)^TR_t^{-1}(x_t-A_tx_{t-1}-B_tu_t)+\frac{1}{2}(x_{t-1}-\mu_{t-1})^T{\Sigma}_{t-1}^{-1}(x_{t-1}-\mu_{t-1})\\ &=L_t(x_{t-1},x_t)+L_t(x_t)\\ \end{align}\]

通过巧妙的分解（需计算指数部分的一阶导数 $\frac{\partial L_t}{\partial x_{t-1}}$ 和二阶导数 $\frac{\partial^2 L_t}{\partial x_{t-1}^2}$ ） $L_t=L_t(x_t)+L_t(x_{t-1},x_t)$ 有（类似于 SLAM 的边缘化）

\[\begin{align} \bar{bel}(x_t)&=\mu\exp\{-L_t(x_t)\}\underbrace{\int \exp\{-L_t(x_{t-1},x_t)\}dx_{t-1}}_{const.}\\ &=\mu\exp\{-L_t(x_t)\}\\ \end{align}\]

式中

\[\begin{align} L_t(x_t) &= \frac{1}{2}\mu_{t-1}^T{\Sigma}_{t-1}^{-1}\mu_{t-1}(x_t-B_tu_t)^TR_t^{-1}(x_t-B_tu_t) + \frac{1}{2}\mu_{t-1}^T{\Sigma}_{t-1}^{-1}\mu_{t-1}\\ &+\frac{1}{2}\left(A_t^TR_t^{-1}(x_t-B_tu_t)+{\Sigma}_{t-1}^{-1}\mu_{t-1} \right)^T \left(A_t^TR_t^{-1}A_t +{\Sigma}_{t-1}^{-1}\right)^{-1} \left(A_t^TR_t^{-1}(x_t-B_tu_t)+{\Sigma}_{t-1}^{-1}\mu_{t-1} \right) \end{align}\]

计算一阶导数 $\frac{\partial L_t(x_t)}{\partial x_{t}}$ 和二阶导数 $\frac{\partial^2 L_t(x_t)}{\partial x_{t}^2}$，令一阶导数为零算出 mean，二阶导数的逆是 covariance：

\[\bar{\mu}_t = A_t\mu_{t-1}+B_tu_t\\ \bar{\Sigma}_t = A_t{\Sigma}_{t-1}A_t^{\top}+R_t\\\]

以上是 KF 的预测阶段的推导过程，下面继续推导更重要的更新过程的公式。

更新阶段

预测过程好比是一个开环控制，而更新过程好比闭环反馈控制。根据贝叶斯公式

\[\begin{align} bel(x_t)&=\mu \,\, \underbrace{p(z_t|x_t)}_{\sim N(z_t;C_tx_t,Q_t)} \quad \underbrace{\bar{bel}(x_t)}_{\sim N(x_t;\bar{\mu}_t,\bar{\Sigma}_t)}\\ &=\mu\exp\{-J_t\} \end{align}\]

式中

\[J_t=\frac{1}{2}(z_t-C_tx_t)^TQ_t^{-1}(z_t-C_tx_t)+\frac{1}{2}(x_t-\bar{\mu}_t)^T\bar{\Sigma}_t^{-1}(x_t-\bar{\mu}_t)\]

计算一阶、二阶导数

\[\begin{align} \frac{\partial J}{\partial x_t}&= -C_t^TQ_t^{-1}(z_t-C_tx_t)+\bar{\Sigma}_t^{-1}(x_t-\bar{\mu}_t)\\ \frac{\partial^2 J}{\partial x_t^2}&= C_t^TQ_t^{-1}C_t+\bar{\Sigma}_t^{-1}\\ \end{align}\]

计算二阶导数的逆

\[{\Sigma}_t=(C_t^TQ_t^{-1}C_t+\bar{\Sigma}_t^{-1})^{-1}\]

令一阶导数为零有

\[{\Sigma}_tC_t^TQ_t^{-1}(z_t-C_t\bar\mu_t)=\mu_t-\bar\mu_t\]

定义 Kalman 增益

\[K_t={\Sigma}_tC_t^TQ_t^{-1}\]

故

\[\mu_t=\bar\mu_t+K_t(z_t-C_t\bar\mu_t)\]

我们发现上面的 $K_t, {\Sigma}_t$ 公式与 KF 的公式不同（KF的 ${\Sigma}_t$ 无须矩阵求逆）。但本质上是一样的

\[\begin{align} K_t &=\cdots= \bar{\Sigma}_tC_t^{\top}\left(C_t\bar{\Sigma}_tC_t^{\top}+Q_t\right)^{-1}\\ {\Sigma}_t&=\cdots=(I-K_tC_t)\bar{\Sigma}_t \end{align}\]

以上是 KF 的更新阶段的推导过程。

补充

以上描述了 KF 推导的重要过程，省略了部分数学推导的细节，比如矩阵求逆的常用公式

\[(R+PQP^T)^{-1}=R^{-1}-R^{-1}P(Q^{-1}+P^TR^{-1}P)^{-1}P^TR^{-1}\]

同时需要掌握如何计算矩阵求导，并学会区分 Layout conventions.

在学习中我们要抓住主干，适当忽略细节。同时不要眼高手低！

TODO：用 C++(Eigen)/MATLAB/Python 实现 KF 算法！

References
  1. Probabilistic Robotics