卡尔曼滤波算法原理
1 公式
预测：
X^k=AX^k−1+Bkuk→\hat{X}_k = A \hat{X}_{k-1} + B_{k} \overrightarrow{u_k}X^k=AX^k−1+Bkuk
Pk=APk−1AT+QP_k = A P_{k-1} A^T + QPk=APk−1AT+Q
修正：
Kk′=PkHT(HPkHT+R)−1K_k'=P_kH^T(H P_k H^T + R)^{-1}Kk′=PkHT(HPkHT+R)−1
X^k′=Xk^+Kk′(zk−HkX^k)\hat{X}_k'=\hat{X_k}+K_k'(z_k-H_k\hat{X}_k)X^k′=Xk^+Kk′(zk−HkX^k)
Pk′=Pk−Kk′HPkP_k' = P_k - K_k' H P_kPk′=Pk−Kk′HPk
2 建模
假设一辆汽车在直路上行驶，车内可以通过 GPS 定位获取自己的位置 p（±10m\pm10m±10m)，也可以获取车速 v（±1m/h\pm1m/h±1m/h），同时车里的人会随机加速或减速，也能获得加速度 a（±1m/s2\pm1m/s^2±1m/s2），我们的目的是通过对这些测量值的滤波让位置 p 的误差控制在 ±1m\pm1m±1m 内。（模型中的测量误差均成正太分布）
我们就用 kalman 的原理逐步推导，直观的了解其数学模型。首先，算法中最关键的其实是概率模型，也就是高斯分布（正太分布）：N(x,μ,σ2)=1σ2πe−(x−μ)22σ2N(x,\mu,\sigma^2)=\frac{1}{\sigma \sqrt{2\pi}} e^{-\frac{(x-\mu)^2}{2 \sigma^2}}N(x,μ,σ2)=σ2π1e−2σ2(x−μ)2， kalman filter 最终的估计值也就是一个概率分布，只是给外面使用的是其中的均值。在这个模型当中，我们假设估计值 X^=[pv]\hat{X}= \begin{bmatrix}p \\v \end{bmatrix}X^=[pv] （高斯分布的均值 μ\muμ），对应的估计协方差为 P=[ΣppΣpvΣvpΣvv]P=\begin{bmatrix}\Sigma_{pp} && \Sigma_{pv} \\ \Sigma_{vp} && \Sigma_{vv} \end{bmatrix}P=[ΣppΣvpΣpvΣvv] （高斯分布的方差 σ2\sigma^2σ2）。
注意：σ\sigmaσ 是标准差，Σ\SigmaΣ 是方差，Σ=σ2\Sigma = \sigma^2Σ=σ2。
预测
预测取决于实际的物理模型， 这里用到的就是运动学方程：
pk=pk−1+Δtvk−1p_k = p_{k-1} + \Delta{tv_{k-1}}pk=pk−1+Δtvk−1
vk=vk−1v_k = \quad \quad \quad \quad v_{k-1}vk=vk−1
即：
X^k=[1Δt01]X^k−1=AX^k−1\begin{aligned} \hat{X}_k &= \begin{bmatrix}1 && \Delta{t} \\ 0 && 1 \end{bmatrix} \hat{X}_{k-1} \\ &= A \hat{X}_{k-1} \end{aligned}X^k=[10Δt1]X^k−1=AX^k−1
所以这里的 AAA 就是物理模型矩阵，用于预测出下一时刻的估计值。同样，协方差 PPP 也要经过物理模型的转换，更新协方差的公式如下：
Cov(x)=ΣCov(x) = \SigmaCov(x)=Σ
Cov(Ax)=AΣATCov(Ax) = A\Sigma A^TCov(Ax)=AΣAT
所以预测方程：
X^k=AkX^k−1\hat{X}_k = A_k \hat{X}_{k-1}X^k=AkX^k−1
Pk=APk−1ATP_k = AP_{k-1}A^TPk=APk−1AT
外部控制
上面的预测是在匀速的情况下，没有外部干扰，如果踩油门或刹车带来了加速减速，那么会引入加速度 aaa，同样可以根据运动学方程来描述：
pk=pk−1+Δtvk−1+12aΔt2p_k = p_{k-1} + \Delta{tv_{k-1}} + \frac{1}{2}a \Delta{t^2}pk=pk−1+Δtvk−1+21aΔt2
vk=vk−1+aΔtv_k = \quad \quad \quad \quad v_{k-1} + a \Delta{t}vk=vk−1+aΔt
即：
X^k=AkX^k−1+[Δt22Δt]a=AkXk−1^+Bkuk\begin{aligned} \hat{X}_k &= A_k \hat{X}_{k-1} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} a \\ &= A_k \hat{X_{k-1}} + B_k u_k \end{aligned}X^k=AkX^k−1+[2Δt2Δt]a=AkXk−1^+Bkuk
这里的 BkB_kBk 就是外部控制矩阵，描述了外部控制量与估计值的物理关系，uku_kuk 就是外部控制量。同时，外部控制量带来的影响也是呈高斯分布，其协方差为 QQQ，需要叠加到已预测的协方差当中，所以最终的预测方程组为：
X^k=AX^k−1+Bkuk\hat{X}_k = A \hat{X}_{k-1} + B_{k}u_kX^k=AX^k−1+Bkuk
Pk=APk−1AT+QP_k = A P_{k-1} A^T + QPk=APk−1AT+Q
修正
完成预测之后，我们需要通过传感器的测量值对其进行校正，由于预测值和测量值都是一个估计，都存在自己的 μ\muμ 和 σ2\sigma^2σ2，修正这个环节就是融合两个估计值，得到新的 μ′\mu'μ′ 和 σ′2\sigma'^2σ′2，即滤波器的最优估计：X^x′\hat{X}_x'X^x′、Pk′P_k'Pk′。如何融合两个高斯分布？方法很简单，直接把他们相乘，做归一化（概率和为1）。
先使用一维高斯分布来分析，假设两个分布：N(x,μ0,σ02)N(x,\mu_0,\sigma_0^2)N(x,μ0,σ02) 和 N(x,μ1,σ12)N(x,\mu_1,\sigma_1^2)N(x,μ1,σ12)，使：
N(x,μ0,σ02)⋅N(x,μ1,σ12)=N(x,μ′,σ′2)N(x,\mu_0,\sigma_0^2) \cdot N(x,\mu_1,\sigma_1^2) = N(x,\mu',\sigma'^2)N(x,μ0,σ02)⋅N(x,μ1,σ12)=N(x,μ′,σ′2)
将 N(x,μ,σ2)=1σ2πe−(x−μ)22σ2N(x,\mu,\sigma^2)=\frac{1}{\sigma \sqrt{2\pi}} e^{-\frac{(x-\mu)^2}{2 \sigma^2}}N(x,μ,σ2)=σ2π1e−2σ2(x−μ)2 代入上式且做归一化，得到：
μ′=μ0+σ02(μ1−μ0)σ02+σ12\mu'=\mu_0 + \frac{\sigma_0^2 (\mu_1 - \mu_0)}{\sigma_0^2 + \sigma_1^2}μ′=μ0+σ02+σ12σ02(μ1−μ0)
σ′2=σ02−σ04σ02+σ12\sigma'^2 = \sigma_0^2 - \frac{\sigma_0^4}{\sigma_0^2 + \sigma_1^2}σ′2=σ02−σ02+σ12σ04
把上式相同的部分提取出来，得到：
k=σ02σ02+σ12k = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2}k=σ02+σ12σ02
μ′=μ0+k(μ1−μ0)\mu' = \mu_0 + k(\mu_1 - \mu_0)μ′=μ0+k(μ1−μ0)
σ′2=σ02−kσ02\sigma'^2 = \sigma_0^2 - k \sigma_0^2σ′2=σ02−kσ02
好，得到这个关系式基本就快结束了，我们再看需要融合的两个估计：
- 预测估计：N(x,HX^k,HPkHT)N(x, H \hat{X}_k, HP_kH^T)N(x,HX^k,HPkHT)，即 μ0=HkX^k\mu_0 = H_k \hat{X}_kμ0=HkX^k ， σ02=HPkHT\sigma_0^2 = HP_kH^Tσ02=HPkHT
- 测量估计：N(x,zk,R)N(x, z_k, R)N(x,zk,R)，即 μ1=zk\mu_1 = z_kμ1=zk ， σ12=R\sigma_1^2 = Rσ12=R
在上式中，你可能存在疑问，为什么预测估计中要加入 HHH 转换矩阵？其实我们做融合的前提是两个估计处于同一尺度，例如一个估计是预测的路程，一个估计是测量的耗油量，它们在数值没有直接的关联，融合之后也不存在意义，所以要把路程换算成耗油量再去做融合才有意义，融合之后的值就是耗油量的最优估计，这里的转换就是 HHH，称之为测量转换矩阵，把预测值的尺度转换为测量值的尺度。你可能还会有疑问，我们建模中的预测和测量是同一尺度啊？是的，同一尺度给 HHH 赋值为 1 就可以了。
我们把两个估计代入高斯融合的关系式，得到：
K=HPkHTHPkHT+RK = \frac{HP_kH^T}{HP_kH^T + R}K=HPkHT+RHPkHT
HX^k′=HX^k+K(zk−HX^k)H\hat{X}_k' = H \hat{X}_k + K(z_k - H \hat{X}_k)HX^k′=HX^k+K(zk−HX^k)
HPk′HT=HPkHT−KHPkHTHP'_kH^T = HP_kH^T - K H P_k H^THPk′HT=HPkHT−KHPkHT
化简上面三个式子，等式两边同时左乘 HTH^THT：
HTK=PkHTHPkHT+RH^TK = \frac{P_kH^T}{HP_kH^T + R}HTK=HPkHT+RPkHT
X^k′=X^k+HTK(zk−HX^k)\hat{X}_k' = \hat{X}_k + H^T K(z_k - H \hat{X}_k)X^k′=X^k+HTK(zk−HX^k)
Pk′=Pk−HTKHPkP'_k = P_k - H^T K H P_kPk′=Pk−HTKHPk
使 Kk′=HTKK'_k = H^T KKk′=HTK， 所以最终的修正公式为：
Kk′=PkHT(HPkHT+R)−1K_k'=P_kH^T(H P_k H^T + R)^{-1}Kk′=PkHT(HPkHT+R)−1
X^k′=Xk^+Kk′(zk−HkX^k)\hat{X}_k'=\hat{X_k}+K_k'(z_k-H_k\hat{X}_k)X^k′=Xk^+Kk′(zk−HkX^k)
Pk′=Pk−Kk′HPkP_k' = P_k - K_k' H P_kPk′=Pk−Kk′HPk
大功告成！