卡尔曼滤波器三：卡尔曼公式推导
转载来源：卡尔曼滤波：从入门到精通
简述
考虑一个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时刻下，小车运动后在其他位置上的概率分布也可以用全概率公式表达出来。当然，最后的计算结果还需要进行归一化处理。
如果我们不断地减小每个方格的分辨率，并按照高斯分布给予每个方格一个概率值，并对小车运动也做如此的离散化处理，应该也是可以不断逼近连续的情况（个人猜想）。