状态估计问题是指，基于初始状态信息，一系列观测量，一系列输入量，以及系统的运动模型和观测模型，来计算系统在某时刻的真实状态的估计值。卡尔曼滤波及其相关卡尔曼滤波算法是状态估计的重要方法。本文介绍卡尔曼滤波(Kalman Filter)，扩展卡尔曼滤波(Extended Kalman Filter)。
1. 卡尔曼滤波
1.1. 线性高斯系统
卡尔曼滤波是线性高斯系统的最优无偏估计，定义离散线性高斯系统： \[\left\{\begin{array}{l} 运动方程:\quad x_k=A_kx_{k-1}+B_ku_k+w_k\\ 测量方程:\quad z_k=C_kx_k+v_k \end{array}\tag{1}\right.\] 其中矩阵 \(A_k\) 为转移矩阵（transition matrix），设矩阵 \(B_k=I\) 为控制矩阵，矩阵 \(C_k\) 为观测矩阵(observation matrix)。并且所有状态和噪声均满足高斯分布： \[\begin{align} 过程噪声: \quad & w_k \sim N(0,Q_k)\\ 测量噪声: \quad & v_k \sim N(0,R_k) \end{align}\] 卡尔曼滤波估计线性高斯系统的状态分为两个步骤：
- 预测（Predict）
计算先验： \[\begin{align} \bar{x}_ k &=A_ k\hat{x}_ {k-1}+u_ k \tag{2}\\ \bar{P}_ k &=A_ k\hat{P}_ {k-1}A_ k^T+Q_ k \tag{3} \end{align}\] - 更新（Update）
先计算卡尔曼增益： \[K_k=\bar{P}_kC_k^T(C_k\bar{P}_kC_k^T+R_k)^{-1} \tag{4}\] 再计算后验概率分布： \[\begin{align} \hat{x}_k &=\bar{x}_k+K(z_k-C_k\bar{x}) \tag{5}\\ \hat{P}_k &=(I-KC_k)\bar{P}_k \tag{6} \end{align}\]
以下通过三种方式来推导出卡尔曼滤波器。
1.2. 通过 MAP/贝叶斯推断推导[1][2][3]
状态估计问题的概率解释就是用 \(0\) 到 \(k\) 的数据（包括初始状态，观测量，输入量）来估计当前时刻的状态分布：\(P(x_k\vert x_0,u_{1:k},z_{1:k})\)。根据贝叶斯法则： \[P(x_k\vert x_0,u_{1:k},z_{1:k}) \propto P(z_k\vert x_k)P(x_k\vert x_0,u_{1:k},z_{1:k-1})\tag{7}\] 这三项分别为后验概率，似然，先验概率。所以状态估计可转换为该后验概率最大化（Maximize a Posterior，MAP）问题。MAP 相当于最大化似然与先验的乘积。似然由测量方程给出，先验有运动方程给出。先验部分如果考虑历史所有信息，那么可以用非线性优化框架来解；如果只考虑一阶马尔科夫性，那么就是卡尔曼滤波方法，前述线性高斯系统就满足一阶马尔科夫性。
该系统下，假设已知 \(k-1\) 时刻的后验状态估计 \(\hat{x}_ {k-1}\) 及其协方差 \(\hat{P}_ {k-1}\)，现在要根据 \(k\) 时刻的输入和观测数据，确定 \(x_k\) 的后验分布。这里以 \((\hat{\cdot})\) 表示后验分布，\((\bar{\cdot})\) 表示先验分布。
卡尔曼滤波器首先通过(1)中的运动方程确定 \(x_k\) 的先验分布，即预测过程。将 \(k-1\) 时刻的分布通过运动方程传递，对于均值有： \[\begin{align}
\bar{x}_k &=E[x_k]=E[A_kx_{k-1}+u_k+w_k]\\
&=A_kE[x_{k-1}]+u_k+E[w_k]\\
&=A_k\hat{x}_{k-1}+u_k
\end{align}\] 对于协方差有： \[\begin{align}
\bar{P}_k &=E\left[(x_k-E[x_k])(x_k-E[x_k])^T\right]\\
&=E\left[(A_kx_{k-1}+u_k+w_k-A_k\hat{x}_{k-1}-u_k)\cdot(A_kx_{k-1}+u_k+w_k-A_k\hat{x}_{k-1}-u_k)^T\right]\\
&=A_kE\left[(x _{k-1}-\hat{x} _{k-1})\cdot(x _{k-1}-\hat{x} _{k-1})^T\right]A_k^T+E[w_kw_k^T]\\
&=A_k\hat{P} _{k-1}A _{k-1}^T+Q _k
\end{align}\]
由此可得预测过程： \[\begin{align}
&P(x_k\vert x_0,u_{1:k},z_{1:k-1})=N\left(A_k\hat{x}_{k-1}+u_k,A_k\hat{P}_{k-1}A_k^T+Q_k\right)\tag{8}\\
\iff &公式 (2),(3)
\end{align}\] 另一方面，通过(1)中的观测方程，可以得到在某个状态下观测数据应该为： \[P(z_k\vert x_k)=N(C_kx_k,R)\tag{9}\] 由公式(7)可知，状态的后验概率分布由预测量以及测量量融合得到，这个融合的过程是两个高斯状的概率分布进行相乘，即 \(x_k\) 的后验概率： \[N(\hat{x}_k,\hat{P}_k)=N(C_kx_k,R)\cdot N(\bar{x}_k,\bar{P}_k)\tag{10}\] 比较该式指数部分即可得到更新过程： \[\begin{align}
& (x_k-\hat{x}_k)^T\hat{P}_k^{-1}(x_k-\hat{x}_k)=(z_k-C_kx_k)^TR^{-1}(z_k-C_kx_k)+(x_k-\bar{x}_k)^T\bar{P}_k^{-1}(x_k-\bar{x}_k)\\
\iff &
\left\{\begin{array}{l}
二次项系数:\quad \hat{P}_k^{-1}=C_k^TR^{-1}C_k+\bar{P}_k^{-1}\\
一次项系数:\quad 2\hat{x}_k^T\hat{P}_k^{-1}x_k=2z_k^TR^{-1}C_kx_k+2\bar{x}_k^T\bar{P}_k^{-1}x_k
\end{array}\right. \tag{11} \\
\iff &
\left\{\begin{array}{l}
I=\hat{P}_kC_k^TR^{-1}C_k+\hat{P}_k\bar{P}_k^{-1}\\
\hat{x}_k=\hat{P}_kC_k^TR^{-1}z_k+\hat{P}_k\bar{P}_k^{-1}\bar{x}_k
\end{array}\right. 令 K=\hat{P}_kC_k^TR^{-1} \\
\iff &
\left\{\begin{array}{l}
I=KC_k+\hat{P}_k\bar{P}_k^{-1}\\
\hat{x}_k=Kz_k+(I-KC_k)\bar{x}_k
\end{array}\right. \\
\iff & 式 (4),(5),(6)
\end{align}\] 对于更新过程，[3]中提出了另一种更加形象的证明方法。如图1所示，容易得到小车模型的运动方程： \[\begin{bmatrix}
x _k\\
\dot{x} _k\\
\end{bmatrix}=
\begin{bmatrix}
1 & \Delta k\\
0 & 1\\
\end{bmatrix}
\begin{bmatrix}
x _{k-1}\\
\dot{x} _{k-1}\\
\end{bmatrix}+
\begin{bmatrix}
\frac{(\Delta k)^2}{2}\\
\Delta k\\
\end{bmatrix}a_k
\] 其中 \(a_k\) 为加速度输入量，对比式(1)也容易得到转移矩阵与控制矩阵。预测过程的证明方式与上述一致，下面简述其更新过程的证明，详见[3]。 
如图1所示，红色区域代表预测量 \({x}_k\) 的概率分布高斯函数；蓝色代表测量量 \(z_k\) 概率分布的高斯函数，测量装置为左侧的 ToF 装置，单位为秒。绿色代表状态的后验概率分布 \(_k\)，由预测量的概率(先验)与测量量的概率(似然)相乘得到。由式(10)可知，两个高斯函数相乘还是高斯函数（但是是尺度变化的高斯函数，Scaled Gaussian[4]），上面的证明过程直接比较二次项与一次项，这里是直接写出新的高斯分布均值方差与另两个高斯分布均值方差的关系，本质上都是比较自变量前面的系数，非系数是不相等的，还有 Scaled 项。由此可得到更新过程。要注意的是，高斯分布相乘时，要注意单位的转换(即需要满足式(10)的单位形式)，这里的观察矩阵就是基于测量装置的测量单位(秒)与状态单位(米，米/秒)的转换值。
2. 扩展卡尔曼滤波
2.1. 非线性非高斯系统
通常系统（如 SLAM）的运动方程和观测方程是非线性函数，写成一般形式： \[\left\{\begin{array}{l} 运动方程:\quad x_k=f(x_{k-1},u_k)+w_k\\ 测量方程:\quad z_k=h(x_k)+v_k \end{array}\tag{12}\right.\] 扩展卡尔曼滤波估计非线性系统的状态与卡尔曼滤波类似，也分为两个步骤：
- 预测（Predict） 计算先验： \[\begin{align} \bar{x} _k&=f(\hat{x} _{k-1},u _k) \tag{13}\\ \bar{P} _k&=F\hat{P} _kF^T+Q _k \tag{14} \end{align}\]
- 更新（Update） 先计算卡尔曼增益： \[K_k=\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1} \tag{15}\] 再计算后验概率分布： \[\begin{align} \hat{x}_k &=\bar{x}_k+K(z_k-h(\bar{x})) \tag{16}\\ \hat{P}_k &=(I-KH_k)\bar{P}_k \tag{17} \end{align}\]
2.2. 通过 MAP/贝叶斯推断推导[1][2]
在某个点附件考虑运动方程与观测方程的一阶泰勒展开，只保留一阶项，即线性部分，然后按照线性系统进行推导。在 \(k\) 时刻，将运动方程和观测方程在 \(\hat{x}_ {k-1},\hat{P}_ {k-1}\) 处进行线性化： \[\left\{\begin{array}{l}
运动方程:\quad x_k\approx f(\hat{x}_{k-1},u_k)+F(x_{k-1}-\hat{x}_{k-1})+w_k\\
测量方程:\quad z_k\approx h(\bar{x}_k)+H(x_k-\bar{x}_k)+v_k
\end{array}\tag{18}\right.\] 其中 \(F=\left.\frac{\partial f}{\partial x_{k-1}}\right\arrowvert_{\hat{x}_ {k-1}}\)， \(H=\left.\frac{\partial h}{\partial x_k}\right\arrowvert_{\bar{x}_ k}\)。
由此可得预测过程: \[\begin{align}
&P(x_k\vert x_0,u_{1:k},z_{1:k-1})=N\left(f(\hat{x}_{k-1},u_k),F\hat{P}_{k-1}F^T+Q_k\right)\tag{19}\\
\iff &公式 (13),(14)
\end{align}\] 另一方面，通过(18)中的观测方程，可以得到在某个状态下观测数据应该为： \[P(z_k\vert x_k)=N(h(\bar{x})+H(x_k-\bar{x}_k),R)\tag{20}\] 由贝叶斯公式，可得 \(x_k\) 的后验概率： \[N(\hat{x}_k,\hat{P}_k)=N(h(\bar{x})+H(x_k-\bar{x}_k),R))\cdot N(\bar{x}_k,\bar{P}_k)\tag{21}\] 类似卡尔曼推导过程，由此可得到更新过程式(15)，(16)，(17)。
[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] T. D. Barfoot. State Estimation for Robotics. Cambridge University Press, 2017.
[3] Faragher, Ramsey. "Understanding the basis of the Kalman filter via a simple and intuitive derivation." IEEE Signal processing magazine 29.5 (2012): 128-132.
[4] Bromiley, Paul. "Products and convolutions of Gaussian probability density functions." Tina-Vision Memo 3.4 (2003): 1.
