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​+Bk​uk​​

Pk=APk−1AT+QP_k = A P_{k-1} A^T + QPk​=APk−1​AT+Q

修正：
Kk′=PkHT(HPkHT+R)−1K_k'=P_kH^T(H P_k H^T + R)^{-1}Kk′​=Pk​HT(HPk​HT+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​−Hk​X^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π​1​e−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​=Ak​X^k−1​

Pk=APk−1ATP_k = AP_{k-1}A^TPk​=APk−1​AT

外部控制

上面的预测是在匀速的情况下，没有外部干扰，如果踩油门或刹车带来了加速减速，那么会引入加速度 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​+21​aΔ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​​=Ak​X^k−1​+[2Δt2​Δt​]a=Ak​Xk−1​^​+Bk​uk​​

这里的 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​+Bk​uk​

Pk=APk−1AT+QP_k = A P_{k-1} A^T + QPk​=APk−1​AT+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π​1​e−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​,HPk​HT)，即 μ0=HkX^k\mu_0 = H_k \hat{X}_kμ0​=Hk​X^k​ ， σ02=HPkHT\sigma_0^2 = HP_kH^Tσ02​=HPk​HT
• 测量估计：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=HPk​HT+RHPk​HT​

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=HPk​HT−KHPk​HT

化简上面三个式子，等式两边同时左乘 HTH^THT：
HTK=PkHTHPkHT+RH^TK = \frac{P_kH^T}{HP_kH^T + R}HTK=HPk​HT+RPk​HT​

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′​=Pk​HT(HPk​HT+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​−Hk​X^k​)

Pk′=Pk−Kk′HPkP_k' = P_k - K_k' H P_kPk′​=Pk​−Kk′​HPk​

大功告成！