卡尔曼滤波 (Kalman Filter)
卡尔曼滤波 (Kalman Filter)是一种基于满足高斯分布的观测量和过程量进行校正的滤波方法。
Agreement
对于离散控制系统，假设其状态向量为 $X$，控制输入为 $u$，过程噪声以 $e\sim N\left(\vec{0},Q’\right)$ 表示，观测向量为 $Z$，则 $$ \begin{align} X_k &= f\left(X_{k-1},u_{k-1},e_{k-1}\right) \tag{1} \\ Z_k &= h\left(X_k\right) \tag{2} \end{align} $$ 其中
- 随机向量 $e$ 表征影响模型的独立随机因素，通常认为它们服从零均值正态分布且相互独立，因此可认为 $Q’$ 为对角矩阵，且对角元素对应各因素的方差；
- 函数 $f()$ 是状态向量的转移模型，它并不总是线性的；
- 函数 $h()$ 是状态向量与观测向量之间的关系，它也并不总是线性的；
Kalman Filter
Intro
就我的理解，卡尔曼滤波的实质是利用高斯概率密度函数的乘积仍近似为高斯概率密度函数。对于随机变量 $X\sim N\left(\mu_1,\sigma_1^2\right)$，和 $Y\sim N\left(\mu_2,\sigma_2^2\right)$，则 $Z=XY\sim N\left(\mu,\sigma^2\right)$，其中 $$ \begin{align} \mu &= \frac{\mu_1\sigma_2^2+\mu_2\sigma_1^2}{\sigma_1^2+\sigma_2^2} = \mu_1+\frac{\sigma_1^2\left(\mu_2-\mu_1\right)}{\sigma_1^2+\sigma_2^2} \tag{3}\\ \sigma^2 &= \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} = \sigma_1^2-\frac{\sigma_1^4}{\sigma_1^2+\sigma_2^2} \tag{4} \end{align} $$ 此处可参考文末的 [Products of Gaussian Probability Density Function](#Products of Gaussian Probability Density Function) 。
Infer
当约定中的状态方程和观测方程可以线性表示时， Kalman Filter 就可以派上用场了。令线性方程如下 $$ \begin{align} X_k &= AX_{k-1}+Bu_{k-1}+Ce_{k-1} \tag{5} \\ Z_k &= HX_k \tag{6} \\ \end{align} $$ 则有 $$ \begin{align} E\left(\hat{X_k}\right) &= AE\left(\check{X_{k-1}}\right)+Bu_{k-1} \tag{7} \\ \sum\left(\hat{X_k}\right) &= A\sum\left(\check{X_{k-1}}\right)A^T+Q \tag{8} \\ Q &\overset{def}{=} CQ'C^T \tag{9} \\ E\left(\hat{Z_k}\right) &= HE\left(\hat{X_k}\right) \tag{10} \\ \sum\left(\hat{Z_k}\right) &= H\sum\left(\hat{X_k}\right)H^T \tag{11} \end{align} $$ 其中
- $\sum$ 表示求协方差矩阵；
- $\hat{X}$ 表示预测值，而 $\check{X}$ 表示修正值；
- $Q$ 表示过程噪声产生的协方差矩阵，它由过程噪声 $Q’$ 生成。
如果有测量 $M\sim N\left(z_k,R\right)$，其中 $R$ 为测量噪声协方差。则对于同一随机量存在两个来源的描述，其一来自预测 $N\left(E\left(\hat{Z_k}\right),\sum\left(\hat{Z_k}\right)\right)$，其二来自测量 $N\left(z_k,R\right)$，对它们的概率密度函数相乘（即融合，可参考两单变量正态分布乘积的概率分布图）可以得到同时支持它们的概率密度分布，即 $$ \begin{align} E\left(\check{Z_k}\right) & = HE\left(\check{X_k}\right) &=& E\left(\hat{Z_k}\right)+\frac{\sum\left(\hat{Z_k}\right)\left(z_k-E\left(\hat{Z_k}\right)\right)}{\sum\left(\hat{Z_k}\right)+R} \notag \\ &&=& HE\left(\hat{X_k}\right)+ \frac{H\sum\left(\hat{X_k}\right)H^T\left(z_k-HE\left(\hat{X_k}\right)\right)}{H\sum\left(\hat{X_k}\right)H^T+R} \tag{12} \\ \notag \\ \sum\left(\check{Z_k}\right) & = H\sum\left(\check{X_k}\right)H^T &=& \sum\left(\hat{Z_k}\right)-\frac{\sum\left(\hat{Z_k}\right)\cdot\sum\left(\hat{Z_k}\right)}{\sum\left(\hat{Z_k}\right)+R} \notag \\ &&=& H\sum\left(\hat{X_k}\right)H^T-\frac{H\sum\left(\hat{X_k}\right)H^T\cdot H\sum\left(\hat{X_k}\right)H^T}{H\sum\left(\hat{X_k}\right)H^T+R} \tag{13} \end{align} $$ 化简得 $$ \begin{align} K_k &\overset{def}{=} \frac{\sum\left(\hat{X_k}\right)H^T}{H\sum\left(\hat{X_k}\right)H^T+R}\tag{14} \\ \notag \\ E\left(\check{X_k}\right) &= E\left(\hat{X_k}\right)+\frac{\sum\left(\hat{X_k}\right)H^T\left(z_k-HE\left(\hat{X_k}\right)\right)}{H\sum\left(\hat{X_k}\right)H^T+R} \notag \\ &= E\left(\hat{X_k}\right)+K_k\left(z_k-HE\left(\hat{X_k}\right)\right) \tag{15}\\ \notag \\ \sum\left(\check{X_k}\right) &= \sum\left(\hat{X_k}\right)-\frac{\sum\left(\hat{X_k}\right)H^T\cdot H\sum\left(\hat{X_k}\right)}{H\sum\left(\hat{X_k}\right)H^T+R} \notag \\ &= \sum\left(\hat{X_k}\right)-K_kH\sum\left(\hat{X_k}\right) \notag \\ &= \left(I-K_kH\right)\sum\left(\hat{X_k}\right) \tag{16} \end{align} $$
Conclusion
可以看出使用 Kalman Filter 的流程可以概括为
1. 建立状态方程（状态转移模型），确定`转移矩阵(A)`、`控制矩阵(B)`、`过程噪声矩阵(Q)`
2. 建立观测方程（观测模型），确定`观测矩阵(H)`、`测量噪声(R)`
3. 状态预测
4. 更新状态向量概率分布的数字特征（均值、协方差），即融合
数据流可表示为
Extended Kalman Filter
Intro
对于线性的状态与观测方程可以方便的使用 kalman Filter ，但对于非线性的方程则不能直接使用。其中一种解决思路是利用多元泰勒级数对非线性函数进行一阶展开，这也是EKF与KF的最主要区别。
多元函数一阶偏导数规则排列形成的矩阵也叫
Jacobian矩阵，用符号$J$表示，而二阶偏导数规则排列形成的矩阵叫Hessian矩阵，用符号$H$表示。
Infer
当约定中的状态方程和观测方程非线性时，对它们线性化后可以得到 $$ \begin{align} E\left(\hat{X_k}\right) &= f\left(\sum\check{X_{k-1}},u_{k-1},\vec{0}\right) \tag{17} \\ \notag \\ \sum\left(\hat{X_k}\right) &= J_A\sum\left(\check{X_{k-1}}\right)J_A^T+Q \tag{18} \\ \notag \\ K_k &\overset{def}{=} \frac{\sum\left(\hat{X_k}\right)J_H^T}{J_H\sum\left(\hat{X_k}\right)J_H^T+R}\tag{19} \\ \notag \\ E\left(\check{X_k}\right) &= E\left(\hat{X_k}\right)+K_k\left(z_k-h\left(E\left(\hat{X_k}\right)\right)\right) \tag{20}\\ \notag \\ \sum\left(\check{X_k}\right) &= \left(I-K_kJ_H\right)\sum\left(\hat{X_k}\right) \tag{21} \end{align} $$
Conclusion
可以看出，与KF相比，EKF仅在需要计算协方差时对非线性函数进行了线性化，而在计算期望时采用非线性函数直接计算。
Programming
Jacobian矩阵的计算需使用Python、Matlab等工具的符号求解系统进行验证，博客中的公式存在错误，如 AdamShan的EKF 一节中关于状态转移矩阵的Jacobian计算矩阵第二行存在错误，其后的雷达观测函数Jacobian矩阵 $\dot\rho$ 的计算也存在疑问（对比其下文正确的代码实现），已在aptiv-adas-fusion仓库中做了校正。- 对于涉及到角度差计算的，需注意是否有必要将角度规范化到区间 $\left[-\pi,\pi\right)$ ，如雷达观测量中的
azimuth在计算状态更新期望时一定是要规范化的。 - 过程噪声矩阵的自由度可能跟状态量数量无关。目前我的理解是跟假设的运动模型有关，以
CTRV为例，该模型假定了匀转向率（即 $\omega$ 恒定）、匀速（即 $v$ 恒定），那么建模误差就是由不满足该假设的情形带来，即 $\omega$ 和 $v$ 变化，因此产生了对应的加速度。如果假设 $\dot\omega$ 和 $\dot v$ 独立且同属于高斯分布，那么噪声自由度就是 $2$，再考虑 $\dot\omega$ 和 $\dot v$ 对状态量的关系，即可得到过程噪声矩阵。 - 勿盲目使用网络上的公式！首先需确认公式正确性，其次确定公式表述的是哪些量之间的关系，不同的状态量所需的公式是不同的，且计算出的
Jacobian也会大不相同。 - Python
sympy可用于符号求解Jacobianimport sympy from sympy import sin, cos, sqrt, atan2, init_printing init_printing(use_latex=True) range, azimuth, rangerate, px, py, vv, theta, omega = sympy.symbols("range azimuth rangerate px py vv theta omega") range = sqrt(px**2+py**2) azimuth = atan2(py, px) rangerate = (px*vv*cos(theta)+py*vv*sin(theta))/sqrt(px**2+py**2) funcs = sympy.Matrix([range, azimuth, rangerate]) args = sympy.Matrix([px, py, vv, theta, omega]) result = funcs.jacobian(args) - Python
numdifftools可用于自动求解Jacobianimport numdifftools as nd radar_observe = lambda x: np.vstack(( np.sqrt(x[0]**2+x[1]**2), np.arctan2(x[1], x[0]), (x[0]*x[2]*np.cos(x[3])+x[1]*x[2]*np.sin(x[3]))/np.sqrt(x[0]**2+x[1]**2))) radar_observe_jacobian = nd.Jacobian(radar_observe) H = radar_observe_jacobian(state.ravel().tolist())[:, :, 0]
Unscented Kalman Filter
Intro
对于非线性模型，虽然可以采用EKF那样的线性化方法，但是一方面计算Jacobian矩阵非常耗费资源，另一方面对于非线性模型的近似效果也不尽理想。UKF与EKF相比根本的区别是，EKF是寻找非线性函数的线性近似，而UKF是寻找映射分布的近似正态分布。它通过一定的规则生成规定数量的采样点 (sigma point)，经非线性变换后重新计算出正态分布的期望和协方差。
显见，这里的采样点的选择，以及基于这些点的分布重建是关键。无迹卡尔曼滤波的采样策略目前有多种，包括① 对称采样；② 比例采样修正；等。UKF性能分析及其在组合导航中的应用 1一文中有较详细的介绍，可自行参考，本文仅介绍比例采样修正的采样策略。
1) 采样点生成 如果增广状态维度为 $n$ （其中包含过程噪声的 $m$ 个维度），则需要 $2n+1$ 个采样点。 $$ \begin{equation} x_k^i = \left\{ \begin{array}{lr} E\left(\check{X_k}\right), & i=1; \\ E\left(\check{X_k}\right)+\left(\sqrt{\left(n+\lambda\right)P}\right)_i, & i=2,...,n+1. \tag{22} \\ E\left(\check{X_k}\right)-\left(\sqrt{\left(n+\lambda\right)P}\right)_i, & i=n+2,...,2n+1. \end{array} \right. \\ \end{equation} $$ 其中下标$i$表示取矩阵的 $i$ 列（或 $i$ 行），此外 $$ \begin{align} P &= \left[ \begin{array}{lr} \sum\check{X_{k-1}} & 0 \\ 0 & Q' \end{array} \right] \tag{23} \\ \notag \\ P &= AA^T \Longrightarrow \sqrt{P}=A \tag{24} \end{align} $$ 上式中需要对矩阵开方，通常该操作较为复杂，但是当 $P$ 为对角矩阵时，过程将变得简单。对 $P$ 做 Cholesky分解 (Cholesky decomposition)，取其下三角矩阵即为开方结果 $A$ 。 对于 $n$ 维的增广状态，将生成 $n\times\left(2n+1\right)$ 的采样点矩阵。 2) 采样点的权重
由采样点计算映射后分布的均值权重 $$ \begin{equation} w_m^i = \left\{ \begin{array}{lr} \frac{\lambda}{\lambda+n}, & i=1; \\ \frac{1}{2\left(\lambda+n\right)}, & i=2,...,2n+1. \tag{25} \\ \end{array} \right. \\ \end{equation} $$ 由采样点计算映射后分布的协方差权重 $$ \begin{equation} w_c^i = \left\{ \begin{array}{lr} \frac{\lambda}{\lambda+n}+1+\beta-\alpha^2, & i=1; \\ \frac{1}{2\left(\lambda+n\right)}, & i=2,...,2n+1. \tag{26} \\ \end{array} \right. \\ \end{equation} $$ 而 $\lambda$ 由下式定义 $$ \begin{equation} \lambda = \alpha^2\left(n+k\right)-n \tag{27} \\ \end{equation} $$ 其中，$k$ 值需要保证后验协方差的半正定性，在高斯分布下，对于状态变量为单变量的情况，取 $k=0$，而多变量时取 $k=3-n$。参数 $\alpha$的取值范围为 $0\leq\alpha\leq 1$，而当非线性严重时，常取 $\alpha = 1\times10^{-3}$。参数 $\beta$ 是一个非负的权系数，调节它可以提高协方差的近似精度，对于高斯分布，常取 $\beta = 2$。
三、基于Sigma Point的均值和协方差计算 $$ \begin{align} E &= \sum_{i=1}^{2n+1}w_m^ix_k^i \tag{28} \\ \sum &= \sum_{i=1}^{2n+1}w_c^i\left(x_k^i-E\right)\left(x_k^i-E\right)^T \tag{29} \\ \end{align} $$
Infer
无迹卡尔曼滤波的推导过程较为复杂，个人认为工程中能够实现、应用并进行调参即可。若感兴趣，可以参考 Unscented Filtering and Nonlinear Estimation 2和 Unscented Kalman Filter Tutorial 3。
本节下面梳理UKF的使用方法（生成的Sigma采样点用 $\chi$ 表示，对采样点进行状态预测后用 $\chi_{pred}$ 表示，对预测后的状态观测后用 $\chi_{prop}$ 表示）。
$$
\begin{align}
E\left(\hat{X_k}\right) &=
\sum_{i=1}^{2n+1}w_m^i\chi_{pred}^i \tag{30} \\ \notag \\
\sum\left(\hat{X_k}\right) &=
\sum_{i=1}^{2n+1}w_c^i\left(\chi_{pred}^i-E_x\right)\left(\chi_{pred}^i-E_x\right)^T \tag{31} \\ \notag \\
E\left(\hat{Z_k}\right) &=
\sum_{i=1}^{2n+1}w_m^i\chi_{prop}^i \tag{32} \\ \notag \\
\sum\left(\hat{Z_k}\right) &=
\sum_{i=1}^{2n+1}w_c^i\left(\chi_{prop}^i-E_z\right)\left(\chi_{prop}^i-E_z\right)^T+R \tag{33} \\ \notag \\
\sum\nolimits^{x,z} &=
\sum_{i=1}^{2n+1}w_c^i\left(\chi_{pred}^i-E_x\right)\left(\chi_{prop}^i-E_z\right)^T \tag{34} \\ \notag \\
K_k &\overset{def}{=} \sum\nolimits^{x,z}\cdot\sum\left(\hat{Z_k}\right)^{-1} \tag{35} \\ \notag \\
E\left(\check{X_k}\right) &= E\left(\hat{X_k}\right)+K_k\left(z_k-E\left(\hat{Z_k}\right)\right) \tag{36}\\ \notag \\
\sum\left(\check{X_k}\right) &= \sum\left(\hat{X_k}\right)-K_k\sum\left(\hat{Z_k}\right)K_k^T \tag{37}
\end{align}
$$
需要注意的是，本方法中的过程噪声 $Q$ 是在生成Sigma点时引入。更详细的实现表述可以参考课件。
Conclusion
无迹卡尔曼滤波 (UKF) 的核心就是无迹变换，基于无迹变换的它在近似非线性模型方面拥有比扩展卡尔曼更优越的性能，我所见的自动驾驶项目中大多使用这种方法。
Boost
Products of Gaussian Probability Density Function
Normalized Innovation Squared (NIS)
具体内容可参考文献 Adaptive Filtering for Single Target Tracking 4.
多传感器融合之卡尔曼滤波
据我所知，将 Kalman Filter 或其衍生算法用于多传感器的融合有多种方案，
具体内容参考文献 Multi-sensor optimal information fusion Kalman filter 5
Appendix
Reference
- AdamShan讲KF
- AdamShan讲EKF
- AdamShan讲UKF
- How a Kalman filter works, in pictures
- 图解卡尔曼滤波器工作原理
- AIS之KF概要
- Some Python Implementations of the Kalman Filter
- 高斯分布的乘积
- 直观理解高斯函数相乘
- Products and Convolutions of Gaussian Probability Density Functions
- The Extended Kalman Filter: An Interactive Tutorial for Non-Experts
- 卡尔曼滤波中的噪声协方差矩阵（R和Q）应该怎么取值,和噪声分布之间的关系是什么？
- Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software
- 如何证明两个正态分布的密度函数相乘还是一个正态分布的密度函数？
- 卡尔曼滤波（Kalman Filter）有哪些推导方式？
- what is the variance of a constant matrix times a random vector?
- 从贝叶斯滤波到卡尔曼滤波