卡尔曼滤波器三：卡尔曼公式推导
转载来源：卡尔曼滤波：从入门到精通
简述
考虑一个SLAM 问题，它由一个运动方程 ：
x t = f ( x t − 1 , u t ) + ω t (1) \mathbf{x}{t}=f(\mathbf{x}{t-1},\mathbf{u}_{t}) + \omega_t \tag 1 xt=f(xt−1,ut)+ωt(1)
和一个观测方程 组成：
z t , j = h ( y j , x t ) + v t , j (2) \mathbf{z}{t,j} = h( \mathbf{y}{j},\mathbf{x}{t} ) + v{t,j} \tag 2 zt,j=h(yj,xt)+vt,j(2)
就把它当作一个线性系统吧（非线性系统请看下一讲扩展卡尔曼滤波），并且为了简化推导，忽略路标的下标j，并把路标y 并入到状态向量一起优化，那么运动方程就可以写为：
x t = F t x t − 1 + B t u t + ω t (3) \mathbf{x}{t}=\mathbf{F}{t}\mathbf{x}{t-1}+\mathbf{B}{t}\mathbf{u}{t} + \omega{t} \tag 3 xt=Ftxt−1+Btut+ωt(3)
其中，
- x t {\mathbf{x}_{t} } xt为 t 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息；
- u t \mathbf{u}_{t} ut为运动测量值，如加速度，转向等等；
- F t \mathbf{F}_{t} Ft为状态转换方程，将 t-1 时刻的状态转换至 t 时刻的状态；
- B t \mathbf{B}{t} Bt是控制输入矩阵，将运动测量值 u t \mathbf{u}{t} ut的作用映射到状态向量上；
- ω t \omega_{t} ωt是预测的高斯噪声，其均值为 0，协方差矩阵为 Q t Q_{t} Qt 。
这一步在卡尔曼滤波中也称为预测 (predict)。
类似地，测量方程可以写为：
z t = H t x t + v t (4) \mathbf{z}{t}=\mathbf{H}{t}\mathbf{x}{t} + \mathbf{v}{t} \tag 4 zt=Htxt+vt(4)
其中，
- z t \mathbf{z}_{t} zt为传感器的测量值；
- H t \mathbf{H}_{t} Ht为转换矩阵，它将状态向量映射到测量值所在的空间中；
- v t \mathbf{v}{t} vt为测量的高斯噪声，其均值为0，协方差矩阵为 R t \mathbf{R}{t} Rt。
而卡尔曼滤波就是预测 - 测量 之间不断循环迭代。当然，对于某些情况，如GPS + IMU，由于IMU 测量频率 远比GPS 高，在只有IMU 测量值时，只执行运动更新 ，在有GPS 测量值时再进行测量更新。
运动更新
一个小例子
用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。如下图所示：
状态向量为小车的位置和速度：
x t = [ x t x ˙ t ] (5) \mathbf{x}{t}=\left[\begin{array}{c} x{t} \\ \dot{x}_{t} \end{array}\right] \tag 5 xt=[xtx˙t](5)
而司机要是踩了刹车或者油门，小车就会具有一个加速度，
u t = f t m \mathbf{u}{t}=\frac{f{t}}{m} ut=mft
假设 t 和 t-1 时刻之间的时间差为 Δ𝑡 。根据物理知识，有：
{ x t = x t − 1 + x ˙ t − 1 Δ t + 1 2 f t m Δ t 2 x ˙ t = x ˙ t − 1 + f t m Δ t (6) \color{Blue} \begin{cases} x_t = x_{t-1} \quad + &\dot{x}_{t-1}\Delta t &+ &\frac12\frac{f_t}m\Delta t^2 \\ \dot{x}t = &\dot{x}{t-1} &+ &\frac{f_t}m\Delta t\end{cases} \tag 6 {xt=xt−1+x˙t=x˙t−1Δtx˙t−1++21mftΔt2mftΔt(6)
写成矩阵形式：(注意变量对齐 ，提取系数构建矩阵 ，变量构建成向量 )。
[ x t x ˙ t ] = [ 1 Δ t 0 1 ] [ x t − 1 x ˙ t − 1 ] + [ Δ t 2 2 Δ t ] f t m (7) \color{Blue} \begin{bmatrix} x_t \\ \dot{x}{t} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix} \begin{bmatrix} {x}{t - 1} \\ \dot{x}{t - 1} \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \frac{f{t}}m \tag 7 [xtx˙t]=[10Δt1][xt−1x˙t−1]+[2Δt2Δt]mft(7)
跟之前的运动方程对比，就知道
F t = [ 1 Δ t 0 1 ] ， B t = [ Δ t 2 2 Δ t ] (8) \mathbf{F}{t} =\begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}， \mathbf{B}{t} = \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \tag 8 Ft=[10Δt1]，Bt=[2Δt2Δt](8)
公式（3）的估计 形式写为：
x ^ t ∣ t − 1 = F t x ^ t − 1 + B t u t (9) \hat{\mathbf{x}}{t \mid t-1}=\mathbf{F}{t}\hat{\mathbf{x}}{t-1}+\mathbf{B}{t}\mathbf{u}_{t} \tag 9 x^t∣t−1=Ftx^t−1+Btut(9)
x ^ t − 1 \hat{\mathbf{x}}{t-1} x^t−1表示 t-1 时刻卡尔曼滤波的状态估计 ； x ^ t ∣ t − 1 \hat{\mathbf{x}}{t \mid t-1} x^t∣t−1则表示中 t-1 到 t 时刻，预测更新所得的预测值。
再利用运动模型对状态向量进行更新后，还要继续更新状态向量的协方差矩阵 P P P，公式为：
P t ∣ t − 1 = F t P t − 1 F t T + Q t (10) \mathbf{P}{t \mid t-1}=\mathbf{F}{t}\mathbf{P}{t-1}\mathbf{F}{t}^T +\mathbf{Q}_{t} \tag {10} Pt∣t−1=FtPt−1FtT+Qt(10)
假设 x t {\mathbf{x}{t} } xt为 t 时刻下状态向量的真值（自然是永远未知的），由之前的现形运动方程（式(3)）给出，将式(3) 与式(9) 相减可得：
x t − x ^ t ∣ t − 1 = F t ( x t − x ^ t − 1 ) + ω t (11) \mathbf{x}{t} - \hat{\mathbf{x}}{t \mid t-1}=\mathbf{F}{t}(\mathbf{x}{t} - \hat{\mathbf{x}}{t-1})+ \omega_{t} \tag{11} xt−x^t∣t−1=Ft(xt−x^t−1)+ωt(11)
而协方差 ：
P t ∣ t − 1 = E [ ( x t − x ^ t ∣ t − 1 ) ( x t − x ^ t ∣ t − 1 ) T ] = E [ ( F ( x t − 1 − x ^ t ∣ t − 1 ) + ω t ) ⋅ ( F ( x t − 1 − x ^ t ∣ t − 1 ) + ω t ) T ] = F E [ ( x t − 1 − x ^ t ∣ t − 1 ) ⋅ ( x t − 1 − x ^ t ∣ t − 1 ) T ] F T + F E [ ( x t − 1 − x ^ t ∣ t − 1 ) ω t T ] F T + F E [ ω t ( x t − 1 − x ^ t ∣ t − 1 ) T ] F T + E [ ω t ω t T ] (12) \begin{aligned} \mathbf{P}{t|t-1}& =E[(\mathbf{x}t-\mathbf{\hat{x}}{t|t-1})(\mathbf{x}t-\mathbf{\hat{x}}{t|t-1})^T] \\ &=E[(\mathbf{F}(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})+\omega_t)\cdot(\mathbf{F}(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})+\omega_t)^T] \\ &=\mathbf{F}E[(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})\cdot(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})^T]\mathbf{F}^T \\ &+\mathbf{F}E[(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})\omega_t^T]\mathbf{F}^T \\ &+\mathbf{F}E[\omega_t(\mathbf{x}{t-1}-\mathbf{\hat{x}}_{t|t-1})^T]\mathbf{F}^T \\ &+E[\omega_t\omega_t^T] \end{aligned} \tag{12} Pt∣t−1=E[(xt−x^t∣t−1)(xt−x^t∣t−1)T]=E[(F(xt−1−x^t∣t−1)+ωt)⋅(F(xt−1−x^t∣t−1)+ωt)T]=FE[(xt−1−x^t∣t−1)⋅(xt−1−x^t∣t−1)T]FT+FE[(xt−1−x^t∣t−1)ωtT]FT+FE[ωt(xt−1−x^t∣t−1)T]FT+E[ωtωtT](12)
考虑到状态向量和噪声是不相关 的，则 E [ ω t ( x t − 1 − x ^ t ∣ t − 1 ) T ] = 0 \color{Blue}E[\omega_t(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})^T] = 0 E[ωt(xt−1−x^t∣t−1)T]=0 ，上式(12)就可以简化为
P t ∣ t − 1 = F E [ ( x t − 1 − x ^ t ∣ t − 1 ) ⋅ ( x t − 1 − x ^ t ∣ t − 1 ) T ] F T + E [ ω t ω t T ] = F t P t − 1 F t T + Q t (13) \begin{aligned} \mathbf{P}{t|t-1}& =\mathbf{F}E[(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})\cdot(\mathbf{x}{t-1}-\mathbf{\hat{x}}{t|t-1})^T]\mathbf{F}^T +E[\omega_t\omega_t^T] \\ & ={\color{Blue}\mathbf{F}{t}\mathbf{P}{t-1}\mathbf{F}{t}^T }+ \mathbf{Q}_{t} \end{aligned} \tag{13} Pt∣t−1=FE[(xt−1−x^t∣t−1)⋅(xt−1−x^t∣t−1)T]FT+E[ωtωtT]=FtPt−1FtT+Qt(13)
可以看到，经过预测更新 ，协方差矩阵P 变大了 。这是因为状态转换并不完美 ，而且运动测量值 含有噪声，具有较大的不确定性。
预测更新 实际上相当于"加法 "：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。
上面即为卡尔曼滤波中预测这一步。这一步相对比较直观，推导比测量更新简单，就只在这里详细给出了。
如果得到了测量值，那么我们就可以对状态向量进行测量更新了，对应的公式为
x ^ t = x ^ t ∣ t − 1 + K t ( z t − H t x ^ t ∣ t − 1 ) (14) \color{Blue}\mathbf{\hat{x}}t=\mathbf{\hat{x}}{t|t-1}+\mathbf{K}_t(\mathbf{z}_t-\mathbf{H}t\mathbf{\hat{x}}{t|t-1}) \tag {14} x^t=x^t∣t−1+Kt(zt−Htx^t∣t−1)(14)
P t = P t ∣ t − 1 − K t H t P t ∣ t − 1 (15) \mathbf{P}t=\mathbf{P}{t|t-1}-\mathbf{K}_t\mathbf{H}t\mathbf{P}{t|t-1} \tag{15} Pt=Pt∣t−1−KtHtPt∣t−1(15)
其中
K t = P t ∣ t − 1 H t T ( H t P t ∣ t − 1 H t T + R t ) − 1 (16) \mathbf{K}t=\mathbf{P}{t|t-1}\mathbf{H}_t^T(\mathbf{H}t\mathbf{P}{t|t-1}\mathbf{H}_t^T+\mathbf{R}_t)^{-1} \tag{16} Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1(16)
为卡尔曼增益。
从这里就可以看到，测量更新 显然比预测更新 复杂，难点也集中在这里。下面就给出测量更新的详细推导。
测量更新
一维case
从 t-1 时刻起，小车运动后，经过前面所述的预测更新后，我们就得到了 t 时刻的小车位置的预测估计 ，由于在卡尔曼滤波中，我们使用高斯概率分布 来表示小车的位置 ，因此这个预测的位置 可以写为：
y 1 ( r ; μ 1 , σ 1 ) = 1 2 π σ 1 2 e − ( r − μ 1 ) 2 2 σ 1 2 (17) y_1(r;\mu_1,\sigma_1)=\frac{1}{\sqrt{2\pi\sigma_1^2}}e^{-\frac{(r-\mu_1)^2}{2\sigma_1^2}} \tag{17} y1(r;μ1,σ1)=2πσ12 1e−2σ12(r−μ1)2(17)
为了与前面的通用的推导区别开来，在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来， μ 1 \mu_1 μ1为这个高斯分布的均值， σ 1 \sigma_1 σ1 为方差，而r 为小车的可能位置， y 1 y_1 y1 为某个可能位置 r 的概率分布。
假设在t 时刻，我们通过某测距仪 测得小车距离原点的距离r ，由于测量包含噪声（且在面前我们假设了其为高斯噪声），因此该测量值也可以利用高斯概率分布来表示：
y 2 ( r ; μ 2 , σ 2 ) = 1 2 π σ 2 2 e − ( r − u 2 ) 2 2 σ 2 2 (18) y_2(r;\mu_2,\sigma_2)=\frac{1}{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(r-u_2)^2}{2\sigma_2^2}} \tag{18} y2(r;μ2,σ2)=2πσ22 1e−2σ22(r−u2)2(18)
除了下标外，其余的字母的含义都和上面的式子一样。
如上图琐事，现在在 t 时刻，我们有了两个关于小车位置的估计。而我们所能得到的关于小车位置的最佳估计 就是将预测更新 和测量更新 所得的数据融合 起来，得到一个新的估计 。而这个融合，就是一个简单的"乘法 "，并利用了一个性质：
两个高斯分布的乘积 仍然是高斯分布。
y f u s e d ( r ; μ 1 , σ 1 , μ 2 , σ 2 ) = 1 2 π σ 1 2 e − ( r − u 1 ) 2 2 σ 1 2 ⋅ 1 2 π σ 2 2 e − ( r − u 2 ) 2 2 σ 2 2 = 1 2 π σ 1 2 σ 2 2 e − ( ( r − u 1 ) 2 2 σ 1 2 + ( r − u 2 ) 2 2 σ 2 2 ) (19) \begin{aligned}y_{fused}(r;\mu_1,\sigma_1,\mu_2,\sigma_2)&=\frac1{\sqrt{2\pi\sigma_1^2}}e^{-\frac{(r-u_1)^2}{2\sigma_1^2}}\cdot\frac1{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(r-u_2)^2}{2\sigma_2^2}}\\&=\frac1{\sqrt{2\pi\sigma_1^2\sigma_2^2}}e^{-(\frac{(r-u_1)^2}{2\sigma_1^2}+\frac{(r-u_2)^2}{2\sigma_2^2})}\end{aligned} \tag{19} yfused(r;μ1,σ1,μ2,σ2)=2πσ12 1e−2σ12(r−u1)2⋅2πσ22 1e−2σ22(r−u2)2=2πσ12σ22 1e−(2σ12(r−u1)2+2σ22(r−u2)2)(19)
将上式化简一下：
y f u s e d ( r ; μ f u s e d , σ f u s e d ) = 1 2 π σ f u s e d 2 e − ( r − u f u s e d ) 2 2 σ f u s e d 2 (20) y_{fused}(r;\mu_{fused},\sigma_{fused})=\frac1{\sqrt{2\pi\sigma_{fused}^2}}e^{-\frac{(r-u_{fused})^2}{2\sigma_{fused}^2}} \tag{20} yfused(r;μfused,σfused)=2πσfused2 1e−2σfused2(r−ufused)2(20)
其中， μ f u s e d \mu_{{fused}} μfused为 μ 1 \mu_1 μ1 和 μ 2 \mu_2 μ2 的加权平均 ， σ f u s e d \sigma_{fused} σfused 则是 σ 1 \sigma_1 σ1 和 σ 2 \sigma_2 σ2 的调和平均 的二分一：
μ f u s e d = μ 1 σ 2 2 + μ 2 σ 1 2 σ 1 2 + σ 2 2 = μ 1 + σ 1 ( μ 2 − μ 1 ) σ 1 2 + σ 2 2 (21) \mu_{fused}=\frac{\mu_{1}\sigma_{2}^{2}+\mu_{2}\sigma_{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}}=\mu_{1}+\frac{\sigma_{1}(\mu_{2}-\mu_{1})}{\sigma_{1}^{2}+\sigma_{2}^{2}} \tag {21} μfused=σ12+σ22μ1σ22+μ2σ12=μ1+σ12+σ22σ1(μ2−μ1)(21)
σ f u s e d = 1 1 σ 1 + 1 σ 2 = σ 1 σ 2 σ 1 2 + σ 2 2 = σ 1 2 − σ 1 4 σ 1 2 + σ 2 2 (22) \\\sigma_{fused}=\frac{1}{\frac{1}{\sigma_{1}}+\frac{1}{\sigma_{2}}}=\frac{\sigma_{1}\sigma_{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}}=\sigma_{1}^{2}-\frac{\sigma_{1}^{4}}{\sigma_{1}^{2}+\sigma_{2}^{2}} \tag{22} σfused=σ11+σ211=σ12+σ22σ1σ2=σ12−σ12+σ22σ14(22)
最右边的式子是为了后面的计算而准备的。
本质上，这（高斯分布相乘）就是卡尔曼滤波中测量更新的全部了。
那么， 怎么由上面两个简单的一维的式子得到前一节 x ^ t \hat{\mathbf{x}}_t x^t 和 P t \mathbf{P}_t Pt 呢？一步一步来。
转换矩阵H的引入
在刚刚的一维情况的小例子中，我们其实做了一个隐式的假设 ，即有预测更新得到的位置的概率分布和测距仪所得的测量值具有相同的单位 (unit)，如米 (m)。
但实际情况往往不是这样的，比如，测距仪给出的可能不是距离，而是信号的飞行时间 （由仪器至小车的光的传播时间 ），单位为秒 (s)。这样的话，我们就无法直接如上面一般直接将两个高斯分布相乘了。
此时，就该转换矩阵 H t \mathbf{H_t} Ht 闪亮登场了。由于 r = c ⋅ t r = c\cdot t r=c⋅t，c 为光速。所以此时 H t = 1 c \mathbf{H_t} = \frac{1}{c} Ht=c1（测量方程为 t = r c t = \frac{r}{c} t=cr，可以回去参考一下式(4)）。
预测值就要写为：
y 1 ( s ; μ 1 , σ 1 , c ) = 1 2 π ( σ 1 c ) 2 e − ( s − μ 1 c ) 2 2 ( σ 1 c ) 2 (23) y_1(s;\mu_1,\sigma_1,c)=\frac{1}{\sqrt{2\pi(\frac{\sigma_1}{c})^2}}e^{-\frac{(s-\frac{\mu_1}{c})^2}{2(\frac{\sigma_1}{c})^2}} \tag{23} y1(s;μ1,σ1,c)=2π(cσ1)2 1e−2(cσ1)2(s−cμ1)2(23)
而测量值保持不变：
y 2 ( s ; μ 2 , σ 2 ) = 1 2 π σ 2 2 e − ( s − μ 2 ) 2 2 σ 2 2 (24) y_2(s;\mu_2,\sigma_2)=\frac{1}{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(s-\mu_2)^2}{2\sigma_2^2}} \tag{24} y2(s;μ2,σ2)=2πσ22 1e−2σ22(s−μ2)2(24)
这样，两个高斯概率分布在转换矩阵H 的作用下又在同一个空间下了 。根据前面 μ f u s e d \mu_{fused} μfused 和 σ f u s e d \sigma_{fused} σfused 的公式 （式(21) 和式(22)），可得：
μ f u s e d c = μ 1 c + ( σ 1 c ) 2 ( μ 2 − μ 1 c ) ( σ 1 c ) 2 + σ 2 2 (25) \frac{\mu_{fused}}{c}=\frac{\mu_1}{c}+\frac{(\frac{\sigma_1}{c})^2(\mu_2-\frac{\mu_1}{c})}{(\frac{\sigma_1}{c})^2+\sigma_2^2} \tag{25} cμfused=cμ1+(cσ1)2+σ22(cσ1)2(μ2−cμ1)(25)
将上式两端都乘以c 则可得：
μ f u s e d = μ 1 + σ 1 2 c ( σ 1 c ) 2 + σ 2 2 ⋅ ( μ 2 − μ 1 c ) (26) \mu_{fused}=\mu_1+\frac{\frac{\sigma_1^2}{c}}{(\frac{\sigma_1}{c})^2+\sigma_2^2}\cdot(\mu_2-\frac{\mu_1}{c}) \tag{26} μfused=μ1+(cσ1)2+σ22cσ12⋅(μ2−cμ1)(26)
由于 H = 1 c \mathbf{H} = \frac{1}{c} H=c1（这里转换矩阵H 不随时间变化而变化，所以把下标 t 略去 ），并记
K = H σ 1 2 H σ 1 2 + σ 2 2 (27) K = \frac{H\sigma_1^2}{H\sigma_1^2 + \sigma_2^2} \tag{27} K=Hσ12+σ22Hσ12(27)
，则式（26）可以写为：
μ f u s e d = μ 1 + K ( μ 2 − H μ 1 ) (28) \mu_{fused}=\mu_1+K(\mu_2-H\mu_{1}) \tag{28} μfused=μ1+K(μ2−Hμ1)(28)
类似的有，
σ f u s e d 2 c 2 = ( σ 1 c ) 2 − ( σ 1 c ) 4 ( σ 1 c ) 2 + σ 2 2 (29) \frac{\sigma_{fused}^2}{c^2}=(\frac{\sigma_1}{c})^2-\frac{(\frac{\sigma_1}{c})^4}{(\frac{\sigma_1}{c})^2+\sigma_2^2} \tag{29} c2σfused2=(cσ1)2−(cσ1)2+σ22(cσ1)4(29)
两边乘以 c 2 c^2 c2 有：
σ f u s e d 2 = σ 1 2 − σ 1 2 c ( σ 1 c ) 2 + σ 2 2 ⋅ σ 1 2 c = σ 1 2 − K H σ 1 2 (30) \sigma_{fused}^2=\sigma_1^2-\frac{\frac{\sigma_1^2}{c}}{\left(\frac{\sigma_1}{c}\right)^2+\sigma_2^2}\cdot\frac{\sigma_1^2}{c}=\sigma_1^2-KH\sigma_1^2 \tag{30} σfused2=σ12−(cσ1)2+σ22cσ12⋅cσ12=σ12−KHσ12(30)
推广至高维
到了这一步，这个一维情况下卡尔曼滤波的测量更新步骤就已经彻底讲完了。剩下的就是将这个一维例子推广至高维空间中。其实大家仔细观察一下就会得到答案。
-
μ f u s e d \mu_{fused} μfused 就是 x ^ t \hat{\mathbf{x}}_t x^t ，是测量更新后所得的状态向量；
-
μ 1 \mu_1 μ1就是 x ^ t ∣ t − 1 \hat{\mathbf{x}}_{t|t−1} x^t∣t−1 ，是 t-1 时刻到t 时刻的小车的预测更新（或叫运动更新）后的状态向量；
-
μ 2 \mu_2 μ2是 z t \mathbf{z}_t zt，是测量值；
-
σ f u s e d 2 \sigma_{fused}^2 σfused2在高维空间中为 P t \mathbf{P}_{t} Pt，为测量更新后，状态向量的协方差矩阵；
-
σ 1 2 \sigma_1^2 σ12 在高维空间中就成了协方差矩阵 P t ∣ t − 1 \mathbf{P}_{t|t-1} Pt∣t−1 ，是预测更新后状态向量的协方差矩阵；
-
σ 2 2 \sigma_2^2 σ22 是 R t R_t Rt ，是测量值的协方差矩阵；
-
H H H就是 H t H_t Ht了，高维空间中的转换矩阵。
-
而最重要的，卡尔曼增益 K = H σ 1 2 H σ 1 2 + σ 2 2 \color{Blue}K = \frac{H\sigma_1^2}{H\sigma_1^2 + \sigma_2^2} K=Hσ12+σ22Hσ12，在高维空间中就可以写成式（31），看着很复杂，但仔细对照的话就是把前面相迎的数据替换带入即可。
K t = P t ∣ t − 1 H t ⋅ ( H t P t ∣ t − 1 H t T + R t ) − 1 (31) \mathbf{K}t=\mathbf{P}{t|t-1}\mathbf{H}_t\cdot(\mathbf{H}t\mathbf{P}{t|t-1}\mathbf{H}_t^T+\mathbf{R}_t)^{-1} \tag{31} Kt=Pt∣t−1Ht⋅(HtPt∣t−1HtT+Rt)−1(31)最后，根据式(28) μ f u s e d = μ 1 + K ( μ 2 − H μ 1 ) \mu_{fused}=\mu_1+K(\mu_2-H\mu_{1}) μfused=μ1+K(μ2−Hμ1)就可得跟公式（14）一样的结论如下：
x ^ t = x ^ t ∣ t − 1 + K t ( z t − H t x ^ t ∣ t − 1 ) (32) \mathbf{\hat{x}}t=\mathbf{\hat{x}}{t|t-1}+\mathbf{K}_t(\mathbf{z}_t-\mathbf{H}t\mathbf{\hat{x}}{t|t-1}) \tag {32} x^t=x^t∣t−1+Kt(zt−Htx^t∣t−1)(32)根据公式（30） σ f u s e d 2 = σ 1 2 − K H σ 1 \sigma_{fused}^2=\sigma_1^2-KH\sigma_1 σfused2=σ12−KHσ1就可得公式（15）的结论：
P t = P t ∣ t − 1 − K t H t P t ∣ t − 1 (33) \mathbf{P}t=\mathbf{P}{t|t-1}-\mathbf{K}_t\mathbf{H}t\mathbf{P}{t|t-1} \tag{33} Pt=Pt∣t−1−KtHtPt∣t−1(33)
小结一下
通过这个一维情况的推导，希望能说明卡尔曼滤波就是在给定初始值的情况下，由预测和测量不断迭代、更新状态向量。
-
而预测就是一个"加法"：状态转换和运动预测叠加；
-
测量 则是简单的高斯分布相乘 ，中间引入了一个转换矩阵将测量值 和状态向量映射在同一个代数空间中。
讨论
至此，相信你已经明白了卡尔曼滤波的推导过程。而具体的问题就取决于你的建模了。如在上面的小车的例子中，各个参数如下：
F t = [ 1 Δ t 0 1 ] (34) \mathbf{F}_{t} =\begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix} \tag {34} Ft=[10Δt1](34)
B t = [ Δ t 2 2 Δ t ] (35) \mathbf{B}_{t} = \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \tag {35} Bt=[2Δt2Δt](35)
H t = [ 1 c 0 ] (36) \mathbf{H}_t = \begin{bmatrix} \frac{1}{c} & 0 \end{bmatrix} \tag{36} Ht=[c10](36)
假设该时刻下运动测量值为加速度 a t = f t m a_t=\frac{f_t}{m} at=mft，均值为0，标准差为 σ a \sigma_a σa的高斯分布（标准差可以为经验值或仪器精度。
ω t ∼ N ( 0 , Q t ) , Q t = B t B t T σ a 2 = [ 1 4 Δ t 4 1 2 Δ t 3 1 2 Δ t 3 Δ t 2 ] ⋅ σ a 2 \omega_t\sim N(0,\mathbf{Q}_t)\text{,}\mathbf{Q}_t=\mathbf{B}_t\mathbf{B}_t^T\sigma_a^2=\begin{bmatrix}\frac14\Delta t^4&\frac12\Delta t^3\\\\\frac12\Delta t^3&\Delta t^2\end{bmatrix}\cdot\sigma_a^2 ωt∼N(0,Qt),Qt=BtBtTσa2= 41Δt421Δt321Δt3Δt2 ⋅σa2
对于测量值，同样可以假设 v t ∼ N ( 0 , σ z 2 ) \mathbf{v}_t\sim N(0,\sigma_z^2) vt∼N(0,σz2)，那么 R t = H t H t T σ z 2 = [ 1 c 2 ⋅ σ z 2 ] \mathbf{R}_t=\mathbf{H}_t\mathbf{H}_t^T\sigma_z^2=[\frac1{c^2}\cdot\sigma_z^2] Rt=HtHtTσz2=[c21⋅σz2]
多问个为什么
如果只关心卡尔曼滤波的推导和结果，到这里就可以停止啦。
但推完卡尔曼滤波，我还有几个个为什么。知其然更要知其所以然。下面是我对于自己的疑惑学习、思考得到的解答，而且碍于表达能力，不敢说百分百正确。
首先是对于预测更新 。前面也说到了，预测更新相当于"加法"。这相对好理解一些。在t-1 时刻我们有了对于小车位置的一个估计，根据对小车速度（状态向量之一）、小车的加速度（运动测量值）的建模 ，在辅以时间间隔，自然可以计算出小车在该时间间隔内的位移和速度增量 ，再将之叠加到原有的状态向量上即可。由于建模和测量的过程带有噪声，所以此时小车的位置估计的精度是下降的 （方差增大）。
那么为什么测量更新就是乘法而非加法呢？
因为测量更新的依据是贝叶斯法则 。在有了测量值之后，我们求小车位置的概率分布其实就是在求 P ( x ∣ z ) P(x|z) P(x∣z) 。根据贝叶斯法则有：
P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) (37) P\left ( \mathbf{x} \mid \mathbf{z} \right ) = \frac{P\left ( \mathbf{z} \mid \mathbf{x} \right ) P(x)}{P(\mathbf{z})} \tag{37} P(x∣z)=P(z)P(z∣x)P(x)(37)
P ( x ∣ z ) P(x|z) P(x∣z) 是后验概率。直接求后验概率比较困难（为什么？）。假设就在这个一维的小车例子中，当我们得到一个距离测量值z，那么小车的位置可能是距离原点的-z 或z 的两个点上。对于二维就可能是一个圆，三维则是一个球。此时要精确地知道小车的位置（消除歧义点），一则我们可以继续测量，二则需要额外的信息。这就使得求后验概率比较费时费力。
反观贝叶斯法则的右侧，此时我们已经有了先验概率 P ( x ) P(x) P(x)，这是上一时刻的状态向量的概率分布，并且我们也有了 $P(z|x) $，因为所得的测量值 z t = H t x t + v t \mathbf{z}_t=\mathbf{H}_tx_t + v_t zt=Htxt+vt 表达的就是在当前位置下，我们能得到的测量值，亦即贝叶斯中的似然 。在 P ( z ) P(z) P(z) 为一个常数的情况下，最大化 P ( z ∣ x ) P ( x ) P(z|x)P(x) P(z∣x)P(x) 就得到了最优的 P ( x ∣ z ) P(x|z) P(x∣z)。
既然测量更新 是以贝叶斯公式 为基础，那么反观预测更新，除了前面那个直观的"加法"解释之外，是不是也有一个概率上的解释呢？
连续的高斯分布所表示的小车位置的预测更新我没找到（不好意思），但就离散情况 的话还是有的，就是全概率公式 。以下图为例，假设t-1 时刻，小车的位置分布概率如图所示，到了t 时刻，预测小车向前运动了3米（3个格子），但由于模型的不确定性和噪音，我们不能保证小车精确地向前走了3米，根据概率分布，我们可以假设小车有80%的概率往前走了3米，10%的概率往前走了两米，而另有10%的概率往前走了4米。
那么，在t 时刻，若小车真的运动到了这个位置，其概率分布是怎样的呢？它既有可能是在距离该位置3米远的地方以0.8的概率运动到现在这个位置的，也有可能是以0.1 的概率从2或4米远的地方为初始位置运动到这的，根据全概率公式，可以表达为
P ( z ) = 0.8 ⋅ 0.6 + 0.1 ⋅ 0.2 + 0.1 ⋅ 0.2 = 0.52 P(z) = 0.8 \cdot 0.6 + 0.1 \cdot 0.2 + 0.1 \cdot 0.2 = 0.52 P(z)=0.8⋅0.6+0.1⋅0.2+0.1⋅0.2=0.52
类似地，t时刻下，小车运动后在其他位置上的概率分布也可以用全概率公式表达出来。当然，最后的计算结果还需要进行归一化处理。
如果我们不断地减小每个方格的分辨率，并按照高斯分布给予每个方格一个概率值，并对小车运动也做如此的离散化处理，应该也是可以不断逼近连续的情况（个人猜想）。