失眠网 > 滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导

滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导

时间：2021-01-06 14:07:19

滤波系列（一）卡尔曼滤波算法（KF）

在本文，将给出卡尔曼滤波算法的详细数学推导过程，如果想直接了解卡尔曼滤波算法的应用，请看博客：卡尔曼滤波算法的应用（python代码）或者直接可以调用Python FilterPy包

KF推导

符号说明

图1：系统的概率图模型

其中 X t X_t Xt 表示隐状态， Y t Y_t Yt表示量测，黑色的箭头表示状态转移过程，红色的箭头表示测量过程， P ( X t │ X t − 1 ) P(X_t│X_{t-1}) P(Xt│Xt−1) 为状态转移的概率， P ( Y t │ X t ) P(Y_t│X_t ) P(Yt│Xt)为量测概率。

从概率图模型可以看出，这里有两个假设条件，第一：假设目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 X t X_t Xt 只与上一时刻的状态 X t − 1 X_{t-1} Xt−1 有关；第二：假设观测值相互独立，即观测值 Y t Y_t Yt 只与 t t t 时刻的隐状态 X t X_t Xt 有关。

状态转移过程：

P ( X t │ X t − 1 ) = N ( A X t − 1 + B , Q ) （ 1 ） P(X_t│X_{t-1} )=N(AX_{t-1}+B,Q) \quad（1） P(Xt│Xt−1)=N(AXt−1+B,Q)（1）

等价于：

X t = A X t − 1 + B + ω , ω ∼ N ( 0 , Q ) （ 2 ） X_t=AX_{t-1}+B+ω,ω \sim N(0,Q) \quad（2） Xt=AXt−1+B+ω,ω∼N(0,Q)（2）

ω ω ω 表示过程噪声，它服从均值为0，协方差为 Q Q Q的高斯分布。

量测过程：

P ( Y t │ X t ) = N ( H X t + C , R ) （ 3 ） P(Y_t│X_t )=N(HX_t+C,R) \quad（3） P(Yt│Xt)=N(HXt+C,R)（3）

等价于：

Y t = H X t + C + ν , ν ∼ N ( 0 , R ) （ 4 ） Y_t=HX_t+C+ν,ν \sim N(0,R) \quad（4） Yt=HXt+C+ν,ν∼N(0,R)（4）

ν ν ν 表示量测噪声，它服从均值为0，协方差为 R R R的高斯分布。

公式推导：

X t X_t Xt 的后验概率分布为：

P ( X t │ Y 1 , ⋯ Y t ) = P ( X t , Y 1 , ⋯ Y t ) P ( Y 1 , ⋯ Y t ) （ 5 ） P(X_t│Y_1,⋯Y_t )=\frac{P(X_t,Y_1,⋯Y_t )}{P(Y_1,⋯Y_t )} \quad（5） P(Xt│Y1,⋯Yt)=P(Y1,⋯Yt)P(Xt,Y1,⋯Yt)（5）

P ( Y 1 , ⋯ Y t ) P(Y_1,⋯Y_t ) P(Y1,⋯Yt) 为量测 Y 1 , ⋯ Y t Y_1,⋯Y_t Y1,⋯Yt发生的概率，在给定量测数据的情况下为一个常数，因此：

P ( X t │ Y 1 , ⋯ Y t ) ∝ P ( X t , Y 1 , ⋯ Y t ) = P ( Y t │ X t , Y 1 , ⋯ Y t − 1 ) P ( X t │ Y 1 , ⋯ Y t − 1 ) P ( Y 1 , ⋯ Y t − 1 ) ∝ P ( Y t │ X t ) P ( X t │ Y 1 , ⋯ Y t − 1 ) （ 6 ） P(X_t│Y_1,⋯Y_t )\\ ∝P(X_t,Y_1,⋯Y_t ) \\ =P(Y_t│X_t,Y_1,⋯Y_{t-1} )P(X_t│Y_1,⋯Y_{t-1} )P(Y_1,⋯Y_{t-1} ) \\ ∝P(Y_t│X_t )P(X_t│Y_1,⋯Y_{t-1} ) \quad（6） P(Xt│Y1,⋯Yt)∝P(Xt,Y1,⋯Yt)=P(Yt│Xt,Y1,⋯Yt−1)P(Xt│Y1,⋯Yt−1)P(Y1,⋯Yt−1)∝P(Yt│Xt)P(Xt│Y1,⋯Yt−1)（6）

注:同理 P ( Y 1 , ⋯ Y t − 1 ) P(Y_1,⋯Y_{t-1} ) P(Y1,⋯Yt−1)也是一个常数；同时又由假设可知，观测值相互独立，即观测值 Y t Y_t Yt 只与 t t t时刻的隐状态 X t X_t Xt有关，所以 P ( Y t │ X t , Y 1 , ⋯ Y t − 1 ) = P ( Y t │ X t ) P(Y_t│X_t,Y_1,⋯Y_{t-1} )=P(Y_t│X_t ) P(Yt│Xt,Y1,⋯Yt−1)=P(Yt│Xt)。

P ( Y t │ X t ) P(Y_t│X_t ) P(Yt│Xt)已经化为最简，就是量测方程，所以我们将重点解决 P ( X t │ Y 1 , ⋯ Y t − 1 ) P(X_t│Y_1,⋯Y_{t-1}) P(Xt│Y1,⋯Yt−1)。

注:因为我们已知状态转移方程 P ( X t │ X t − 1 ) P(X_t│X_{t-1} ) P(Xt│Xt−1)，所以我们想把这个表达式引入到上面的表达式里，即 P ( X t , X t − 1 │ Y 1 , ⋯ Y t − 1 ) P(X_t,X_{t-1}│Y_1,⋯Y_{t-1} ) P(Xt,Xt−1│Y1,⋯Yt−1)，同时我们为了等式端相等，就需要把随机变量 X t − 1 X_{t-1} Xt−1积分掉。

P ( X t │ Y 1 , ⋯ Y t − 1 ) = ∫ X t − 1 P ( X t , X t − 1 │ Y 1 , ⋯ Y t − 1 ) d X t − 1 = ∫ X t − 1 P ( X t │ X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) d X t − 1 = ∫ X t − 1 P ( X t │ X t − 1 ) P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) d X t − 1 （ 7 ） P(X_t│Y_1,⋯Y_{t-1} )=\int_{X_{t-1}}{P(X_t,X_{t-1}│Y_1,⋯Y_{t-1} )}dX_{t-1}\\ =\int_{X_{t-1}}{P(X_t│X_{t-1},Y_1,⋯Y_{t-1} )P(X_{t-1}│Y_1,⋯Y_{t-1} )}dX_{t-1} \\=\int_{X_{t-1}}{P(X_t│X_{t-1} )P(X_{t-1}│Y_1,⋯Y_{t-1} )}dX_{t-1}\quad（7） P(Xt│Y1,⋯Yt−1)=∫Xt−1P(Xt,Xt−1│Y1,⋯Yt−1)dXt−1=∫Xt−1P(Xt│Xt−1,Y1,⋯Yt−1)P(Xt−1│Y1,⋯Yt−1)dXt−1=∫Xt−1P(Xt│Xt−1)P(Xt−1│Y1,⋯Yt−1)dXt−1（7）

注:由假设1知，目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 X t X_t Xt只与上一时刻的状态 X t − 1 X_{t-1} Xt−1 有关，因此 P ( X t │ X t − 1 , Y 1 , ⋯ Y t − 1 ) = P ( X t │ X t − 1 ) P(X_t│X_{t-1},Y_1,⋯Y_{t-1})=P(X_t│X_{t-1}) P(Xt│Xt−1,Y1,⋯Yt−1)=P(Xt│Xt−1)。

注: P ( X t , X t − 1 │ Y 1 , ⋯ Y t − 1 ) = P ( X t , X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( Y 1 , ⋯ Y t − 1 ) = P ( X t , X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( Y 1 , ⋯ Y t − 1 ) = P ( X t │ X t − 1 , Y 1 , ⋯ Y t − 1 ) P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) P(X_t,X_{t-1}│Y_1,⋯Y_{t-1} )=\frac{P(X_t,X_{t-1},Y_1,⋯Y_{t-1} )}{P(Y_1,⋯Y_{t-1} ) } \\=\frac{P(X_t,X_{t-1},Y_1,⋯Y_{t-1} )P(X_{t-1},Y_1,⋯Y_{t-1} )}{P(X_{t-1},Y_1,⋯Y_{t-1} )P(Y_1,⋯Y_{t-1} )} \\ =P(X_t│X_{t-1},Y_1,⋯Y_{t-1} )P(X_{t-1}│Y_1,⋯Y_{t-1} ) P(Xt,Xt−1│Y1,⋯Yt−1)=P(Y1,⋯Yt−1)P(Xt,Xt−1,Y1,⋯Yt−1)=P(Xt−1,Y1,⋯Yt−1)P(Y1,⋯Yt−1)P(Xt,Xt−1,Y1,⋯Yt−1)P(Xt−1,Y1,⋯Yt−1)=P(Xt│Xt−1,Y1,⋯Yt−1)P(Xt−1│Y1,⋯Yt−1)

整理一下式子6和式子7，

P ( X t │ Y 1 , ⋯ Y t ) ∝ P ( Y t │ X t ) P ( X t │ Y 1 , ⋯ Y t − 1 ) ∝ P ( Y t │ X t ) ∫ X t − 1 P ( X t │ X t − 1 ) P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) d X t − 1 ( 8 ) P(X_t│Y_1,⋯Y_t )∝P(Y_t│X_t )P(X_t│Y_1,⋯Y_{t-1} ) \\∝P(Y_t│X_t )\int_{X_{t-1}}{P(X_t│X_{t-1} )P(X_{t-1}│Y_1,⋯Y_{t-1} )}dX_{t-1}\quad (8) P(Xt│Y1,⋯Yt)∝P(Yt│Xt)P(Xt│Y1,⋯Yt−1)∝P(Yt│Xt)∫Xt−1P(Xt│Xt−1)P(Xt−1│Y1,⋯Yt−1)dXt−1(8)

这里出现了递归！！！P ( X t │ Y 1 , ⋯ Y t ) P(X_t│Y_1,⋯Y_t ) P(Xt│Y1,⋯Yt) 与 P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) P(X_{t-1}│Y_1,⋯Y_{t-1 }) P(Xt−1│Y1,⋯Yt−1)递归，令 P ( X t │ Y 1 , ⋯ Y t ) = N ( μ ^ t , Σ ^ t ) P(X_t│Y_1,⋯Y_t )=N(\hat{\mu}_t,\hat{\Sigma}_t) P(Xt│Y1,⋯Yt)=N(μ^t,Σ^t),那么 P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) = N ( μ ^ t − 1 , Σ ^ t − 1 ) P(X_{t-1}│Y_1,⋯Y_{t-1})=N(\hat{\mu}_{t-1},\hat{\Sigma}_{t-1}) P(Xt−1│Y1,⋯Yt−1)=N(μ^t−1,Σ^t−1)，令 P ( X t │ Y 1 , ⋯ Y t − 1 ) = N ( μ ˉ t , Σ ˉ t ) P(X_t│Y_1,⋯Y_{t-1})=N(\bar{\mu}_t,\bar{\Sigma}_t) P(Xt│Y1,⋯Yt−1)=N(μˉt,Σˉt)。

由公式7得预测步（利用前t-1个时刻的量测来预测第t个时刻的状态）：

P ( X t │ Y 1 , ⋯ Y t − 1 ) = ∫ X t − 1 P ( X t │ X t − 1 ) P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) d X t − 1 = N ( μ ˉ t , Σ ˉ t ) ( 9 ) P(X_t│Y_1,⋯Y_{t-1} )= \int_{X_{t-1}}{P(X_t│X_{t-1} )P(X_{t-1}│Y_1,⋯Y_{t-1} )}dX_{t-1}=N(\bar{\mu}_t,\bar{\Sigma}_t) \quad (9) P(Xt│Y1,⋯Yt−1)=∫Xt−1P(Xt│Xt−1)P(Xt−1│Y1,⋯Yt−1)dXt−1=N(μˉt,Σˉt)(9)

由公式6得更新步（利用第 t t t个时刻的量测来更新第 t t t个时刻的状态）：

P ( X t │ Y 1 , ⋯ Y t ) = N ( μ ^ t , Σ ^ t ) ∝ P ( Y t │ X t ) P ( X t │ Y 1 , ⋯ Y t − 1 ) ( 10 ) P(X_t│Y_1,⋯Y_t )=N(\hat{\mu}_t,\hat{\Sigma}_t)∝P(Y_t│X_t )P(X_t│Y_1,⋯Y_{t-1} ) \quad(10) P(Xt│Y1,⋯Yt)=N(μ^t,Σ^t)∝P(Yt│Xt)P(Xt│Y1,⋯Yt−1)(10)

t = 1 t=1 t=1 : P ( X 1 │ Y 1 ) = N ( μ ^ 1 , Σ ^ 1 ) P(X_1│Y_1 )=N(\hat{\mu}_1,\hat{\Sigma}_1) P(X1│Y1)=N(μ^1,Σ^1)更新（第一个时刻只有更新，没有预测）

t = 2 t=2 t=2 : P ( X 2 │ Y 1 ) = N ( μ ˉ 2 , Σ ˉ 2 ) P(X_2│Y_1 )=N(\bar{\mu}_2,\bar{\Sigma}_2) P(X2│Y1)=N(μˉ2,Σˉ2) 预测

P ( X 2 │ Y 1 , Y 2 ) = N ( μ ^ 2 , Σ ^ 2 ) P(X_2│Y_1,Y_2 )=N(\hat{\mu}_2,\hat{\Sigma}_2) P(X2│Y1,Y2)=N(μ^2,Σ^2)更新

t = 3 t=3 t=3 : P ( X 3 │ Y 1 , Y 2 ) = N ( μ ˉ 3 , Σ ˉ 3 ) P(X_3│Y_1,Y_2 )=N(\bar{\mu}_3,\bar{\Sigma}_3) P(X3│Y1,Y2)=N(μˉ3,Σˉ3) 预测

P ( X 3 │ Y 1 , Y 2 , Y 3 ) = N ( μ ^ 3 , Σ ^ 3 ) P(X_3│Y_1,Y_2,Y_3 )=N(\hat{\mu}_3,\hat{\Sigma}_3) P(X3│Y1,Y2,Y3)=N(μ^3,Σ^3) 更新

… \dots …

t = t t=t t=t: P ( X t │ Y 1 , ⋯ Y t − 1 ) = N ( μ ˉ t , Σ ˉ t ) P(X_t│Y_1,⋯Y_{t-1} )=N(\bar{\mu}_t,\bar{\Sigma}_t) P(Xt│Y1,⋯Yt−1)=N(μˉt,Σˉt) 预测

P ( X t │ Y 1 , ⋯ Y t ) = N ( μ ^ t , Σ ^ t ) P(X_t│Y_1,⋯Y_t )=N(\hat{\mu}_t,\hat{\Sigma}_t) P(Xt│Y1,⋯Yt)=N(μ^t,Σ^t) 更新

给出一个动态模型的描述(不考虑常数项)：

{ X t = A X t − 1 + ω , ω ∼ N ( 0 , Q ) Y t = H X t + ν , ν ∼ N ( 0 , R ) ( 11 ) \begin{cases} X_t=AX_{t-1}+ω,ω \sim N(0,Q) \\ Y_t=HX_t+ν, ν \sim N(0,R)\\ \end{cases} \quad(11) {Xt=AXt−1+ω,ω∼N(0,Q)Yt=HXt+ν,ν∼N(0,R)(11)

由一阶马尔科夫假设和观测独立性假设知： c o v ( X t − 1 , ω ) = 0 , c o v ( X t − 1 , ν ) = 0 , c o v ( ω , ν ) = 0 cov(X_{t-1},ω)=0,cov(X_{t-1},ν)=0,cov(ω,ν)=0 cov(Xt−1,ω)=0,cov(Xt−1,ν)=0,cov(ω,ν)=0

由递归表达式可知：

P ( X t │ Y 1 , ⋯ Y t − 1 ) = N ( μ ˉ t , Σ ˉ t ) ( 12 ) P(X_t│Y_1,⋯Y_{t-1} )=N(\bar{\mu}_t,\bar{\Sigma}_t) \quad (12) P(Xt│Y1,⋯Yt−1)=N(μˉt,Σˉt)(12)

P ( X t − 1 │ Y 1 , ⋯ Y t − 1 ) = N ( μ ^ t − 1 , Σ ^ t − 1 ) = E [ X t − 1 ] + Δ X t − 1 , Δ X t − 1 ∼ N ( 0 , Σ ^ t − 1 ) ( 13 ) P(X_{t-1}│Y_1,⋯Y_{t-1} )=N(\hat{\mu}_{t-1},\hat{\Sigma}_{t-1})\\=E[X_{t-1} ]+ΔX_{t-1}, ΔX_{t-1} \sim N(0,\hat{\Sigma}_{t-1}) \quad(13) P(Xt−1│Y1,⋯Yt−1)=N(μ^t−1,Σ^t−1)=E[Xt−1]+ΔXt−1,ΔXt−1∼N(0,Σ^t−1)(13)

由状态转移过程可知：

P ( X t │ Y 1 , ⋯ Y t − 1 ) = A X t − 1 + ω = A ( E [ X t − 1 ] + Δ X t − 1 ) + ω = A E [ X t − 1 ] + A Δ X t − 1 + ω = E [ X t ] + Δ X t ( 14 ) P(X_t│Y_1,⋯Y_{t-1})=AX_{t-1}+ω\\=A(E[X_{t-1} ]+ΔX_{t-1})+ω=AE[X_{t-1} ]+AΔX_{t-1}+ω\\=E[X_t ]+ΔX_t \quad(14) P(Xt│Y1,⋯Yt−1)=AXt−1+ω=A(E[Xt−1]+ΔXt−1)+ω=AE[Xt−1]+AΔXt−1+ω=E[Xt]+ΔXt(14)

{ E [ X t ] = A E [ X t − 1 ] = A μ ^ t − 1 Δ X t = A Δ X t − 1 + ω ( 15 ) \begin{cases} E[X_t ]=AE[X_{t-1}]=A\hat{\mu}_{t-1}\\ ΔX_t=AΔX_{t-1}+ω\\ \end{cases} \quad(15) {E[Xt]=AE[Xt−1]=Aμ^t−1ΔXt=AΔXt−1+ω(15)

注： V [ X t ] = E [ ( X t − E [ X t ] ) ( X t − E [ X t ] ) T ] = E [ Δ X t Δ X t T ] V[X_t ]=E[(X_t-E[X_t ]) (X_t-E[X_t ])^T ]=E[ΔX_t ΔX_t^T ] V[Xt]=E[(Xt−E[Xt])(Xt−E[Xt])T]=E[ΔXtΔXtT]。

P ( X t │ Y 1 , ⋯ Y t − 1 ) = N ( μ ˉ t , Σ ˉ t ) = N ( A E [ X t − 1 ] , E [ Δ X t Δ X t T ] ) = N ( A μ ^ t − 1 , E [ Δ X t Δ X t T ] ) ( 16 ) P(X_t│Y_1,⋯Y_{t-1} )=N(\bar{\mu}_t,\bar{\Sigma}_t)\\=N(AE[X_{t-1} ],E[ΔX_t ΔX_t^T ])=N(A\hat{\mu}_{t-1},E[ΔX_t ΔX_t^T ]) \quad(16) P(Xt│Y1,⋯Yt−1)=N(μˉt,Σˉt)=N(AE[Xt−1],E[ΔXtΔXtT])=N(Aμ^t−1,E[ΔXtΔXtT])(16)

E [ Δ X t Δ X t T ] = E [ ( A Δ X t − 1 + ω ) ( A Δ X t − 1 + ω ) T ] = E [ ( A Δ X t − 1 + ω ) ( Δ X t − 1 T A T + ω T ) ] = E [ A Δ X t − 1 Δ X t − 1 T A T + ω ω T ] = A E [ Δ X t − 1 Δ X t − 1 T ] A T + E [ ω ω T ] = A Σ ^ t − 1 A T + Q ( 17 ) E[ΔX_t ΔX_t^T ]=E[(AΔX_{t-1} +ω) (AΔX_{t-1} +ω)^T ] \\=E[(AΔX_{t-1} +ω)(ΔX_{t-1} ^T A^T+ω^T )] =E[AΔX_{t-1} ΔX_{t-1} ^T A^T+ωω^T ] \\=AE[ΔX_{t-1} ΔX_{t-1} ^T ] A^T+E[ωω^T ]=A\hat{\Sigma}_{t-1} A^T+Q \quad(17) E[ΔXtΔXtT]=E[(AΔXt−1+ω)(AΔXt−1+ω)T]=E[(AΔXt−1+ω)(ΔXt−1TAT+ωT)]=E[AΔXt−1ΔXt−1TAT+ωωT]=AE[ΔXt−1ΔXt−1T]AT+E[ωωT]=AΣ^t−1AT+Q(17)

注： c o v ( X t − 1 , ω ) = 0 , c o v ( X t − 1 , ν ) = 0 , c o v ( ω , ν ) = 0 cov(X_{t-1} ,ω)=0,cov(X_{t-1} ,ν)=0,cov(ω,ν)=0 cov(Xt−1,ω)=0,cov(Xt−1,ν)=0,cov(ω,ν)=0。

预测公式：

μ ˉ t = A μ ^ t − 1 ( 18 ) \bar{\mu}_t=A\hat{\mu}_{t-1} \quad(18) μˉt=Aμ^t−1(18)

Σ ˉ t = A Σ ^ t − 1 A T + Q ( 19 ) \bar{\Sigma}_t=A\hat{\Sigma}_{t-1} A^T+Q \quad(19) Σˉt=AΣ^t−1AT+Q(19)

由量测方程知：

P ( Y t │ Y 1 , ⋯ Y t − 1 ) = H X t + ν = H ( A E [ X t − 1 ] + A Δ X t − 1 + ω ) + ν = H A E [ X t − 1 ] + H A Δ X t − 1 + H ω + ν = E [ Y t ] + Δ Y t ( 20 ) P(Y_t│Y_1,⋯Y_{t-1} )=HX_t+ν=H(AE[X_{t-1} ]+AΔX_{t-1}+ω)+ν \\=HAE[X_{t-1} ]+HAΔX_{t-1}+Hω+ν=E[Y_t ]+ΔY_t (20) P(Yt│Y1,⋯Yt−1)=HXt+ν=H(AE[Xt−1]+AΔXt−1+ω)+ν=HAE[Xt−1]+HAΔXt−1+Hω+ν=E[Yt]+ΔYt(20)

{ E [ Y t ] = H A E [ X t − 1 ] = H A μ ^ t − 1 = H μ ˉ t Δ Y t = H A Δ X t − 1 + H ω + ν ( 21 ) \begin{cases} E[Y_t ]=HAE[X_{t-1}]=HA\hat{\mu}_{t-1}=H\bar{\mu}_t\\ ΔY_t=HAΔX_{t-1}+Hω+ν\\ \end{cases} \quad(21) {E[Yt]=HAE[Xt−1]=HAμ^t−1=HμˉtΔYt=HAΔXt−1+Hω+ν(21)

P ( Y t │ Y 1 , ⋯ Y t − 1 ) = N ( H A E [ X t − 1 ] , E [ Δ Y t Δ Y t T ] ) = N ( H μ ˉ t , E [ Δ Y t Δ Y t T ] ) ( 22 ) P(Y_t│Y_1,⋯Y_{t-1} )=N(HAE[X_{t-1} ],E[ΔY_t ΔY_t^T ])\\=N(H\bar{\mu}_t,E[ΔY_t ΔY_t^T ])\quad (22) P(Yt│Y1,⋯Yt−1)=N(HAE[Xt−1],E[ΔYtΔYtT])=N(Hμˉt,E[ΔYtΔYtT])(22)

E [ Δ Y t Δ Y t T ] = E [ ( H A Δ X t − 1 + H ω + ν ) ( H A Δ X t − 1 + H ω + ν ) T ] = E [ ( H A Δ X t − 1 + H ω + ν ) ( Δ X t − 1 T A T H T + ω T H T + ν T ) ] = E [ H A Δ X t − 1 Δ X ( t − 1 ) T A T H T + H ω ω T H T + ν ν T ] = H A E [ Δ X t − 1 Δ X ( t − 1 ) T ] A T H T + H E [ ω ω T ] H T + R = H ( A Σ ^ t − 1 A T + Q ) H T + R = H Σ ˉ t H T + R ( 23 ) E[ΔY_t ΔY_t^T ]=E[(HAΔX_{t-1}+Hω+ν) (HAΔX_{t-1}+Hω+ν)^T ] \\=E[(HAΔX_{t-1}+Hω+ν)(ΔX_{t-1}^T A^T H^T+ω^T H^T+ν^T )] \\=E[HAΔX_{t-1} ΔX_(t-1)^T A^T H^T+Hωω^T H^T+νν^T ] \\=HAE[ΔX_{t-1}ΔX_(t-1)^T ] A^T H^T+HE[ωω^T ] H^T+R \\=H(A\hat{\Sigma}_{t-1} A^T+Q) H^T+R=H\bar{\Sigma}_tH^T+R \quad(23) E[ΔYtΔYtT]=E[(HAΔXt−1+Hω+ν)(HAΔXt−1+Hω+ν)T]=E[(HAΔXt−1+Hω+ν)(ΔXt−1TATHT+ωTHT+νT)]=E[HAΔXt−1ΔX(t−1)TATHT+HωωTHT+ννT]=HAE[ΔXt−1ΔX(t−1)T]ATHT+HE[ωωT]HT+R=H(AΣ^t−1AT+Q)HT+R=HΣˉtHT+R(23)

E [ Δ X t Δ Y t T ] = E [ ( A Δ X t − 1 + ω ) ( H A Δ X t − 1 + H ω + ν ) T ] = E [ ( A Δ X t − 1 + ω ) ( Δ X t − 1 T A T H T + ω T H T + ν T ) ] = E [ A Δ X t − 1 Δ X t − 1 T A T H T + ω ω T H T ] = A E [ Δ X t − 1 Δ X t − 1 T ] A T H T + E [ ω ω T ] H T ] = ( A E [ Δ X t − 1 Δ X t − 1 T ] A T + E [ ω ω T ] ] ) H T = ( A Σ ^ t − 1 A T + Q ) H T = Σ ˉ t H T ( 24 ) E[ΔX_t ΔY_t^T ]=E[(AΔX_{t-1}+ω) (HAΔX_{t-1}+Hω+ν)^T ] \\=E[(AΔX_{t-1}+ω)(ΔX_{t-1}^T A^T H^T+ω^T H^T+ν^T )] \\=E[AΔX_{t-1} ΔX_{t-1}^T A^T H^T+ωω^T H^T ] \\=AE[ΔX_{t-1}ΔX_{t-1}^T]A^T H^T+E[ωω^T ] H^T ] \\=(AE[ΔX_{t-1}ΔX_{t-1}^T]A^T+E[ωω^T ]]) H^T \\=(A\hat{\Sigma}_{t-1} A^T+Q) H^T=\bar{\Sigma}_t H^T\quad (24) E[ΔXtΔYtT]=E[(AΔXt−1+ω)(HAΔXt−1+Hω+ν)T]=E[(AΔXt−1+ω)(ΔXt−1TATHT+ωTHT+νT)]=E[AΔXt−1ΔXt−1TATHT+ωωTHT]=AE[ΔXt−1ΔXt−1T]ATHT+E[ωωT]HT]=(AE[ΔXt−1ΔXt−1T]AT+E[ωωT]])HT=(AΣ^t−1AT+Q)HT=ΣˉtHT(24)

E [ Δ Y t Δ X t T ] = E [ ( H A Δ X t − 1 + H ω + ν ) ( A Δ X t − 1 + ω ) T ] = E [ ( H A Δ X t − 1 + H ω + ν ) ( Δ X t − 1 T A T + ω T ) ] = E [ H A Δ X t − 1 Δ X t − 1 T A T + H ω ω T ] = H E [ A Δ X t − 1 Δ X t − 1 T A T + ω ω T ] = H ( A Σ ^ t − 1 A T + Q ) = H Σ ˉ t ( 25 ) E[ΔY_t ΔX_t^T ]=E[(HAΔX_{t-1}+Hω+ν) (AΔX_{t-1}+ω)^T ] \\=E[(HAΔX_{t-1}+Hω+ν)(ΔX_{t-1}^T A^T+ω^T )] \\=E[HAΔX_{t-1} ΔX_{t-1}^T A^T+Hωω^T ] \\=HE[AΔX_{t-1} ΔX_{t-1}^T A^T+ωω^T ] \\=H(A\hat{\Sigma}_{t-1} A^T+Q)=H\bar{\Sigma}_t\quad (25) E[ΔYtΔXtT]=E[(HAΔXt−1+Hω+ν)(AΔXt−1+ω)T]=E[(HAΔXt−1+Hω+ν)(ΔXt−1TAT+ωT)]=E[HAΔXt−1ΔXt−1TAT+HωωT]=HE[AΔXt−1ΔXt−1TAT+ωωT]=H(AΣ^t−1AT+Q)=HΣˉt(25)

( P ( X t │ Y 1 , ⋯ Y t − 1 ) P ( Y t │ Y 1 , ⋯ Y t − 1 ) ) ) ∼ N ( [ A μ ^ t − 1 H A μ ^ t − 1 ] , [ A Σ ^ t − 1 A T + Q Σ ˉ t H T H Σ ˉ t H Σ ˉ t H T + R ) ] ) ( 26 ) \begin{pmatrix} P(X_t│Y_1,⋯Y_{t-1})\\ P(Y_t│Y_1,⋯Y_{t-1}) )\\ \end{pmatrix} \sim N\left(\begin{bmatrix} A\hat{\mu}_{t-1}\\ HA\hat{\mu}_{t-1}\\ \end{bmatrix}, \begin{bmatrix} A\hat{\Sigma}_{t-1} A^T+Q&\bar{\Sigma}_tH^T\\ H\bar{\Sigma}_t&H\bar{\Sigma}_tH^T+R)\\ \end{bmatrix}\right) \quad(26) (P(Xt│Y1,⋯Yt−1)P(Yt│Y1,⋯Yt−1)))∼N([Aμ^t−1HAμ^t−1],[AΣ^t−1AT+QHΣˉtΣˉtHTHΣˉtHT+R)])(26)

注：给出一个公式：

( x 1 x 2 ) ∼ N ( [ μ 1 μ 2 ] , [ Σ 11 Σ 12 Σ 21 Σ 22 ] ) ( 27 ) \begin{pmatrix} x_1\\ x_2\\ \end{pmatrix} \sim N\left(\begin{bmatrix} μ_1\\ μ_2\\ \end{bmatrix}, \begin{bmatrix} Σ_{11}&Σ_{12}\\ Σ_{21}&Σ_{22}\\ \end{bmatrix}\right) \quad(27) (x1x2)∼N([μ1μ2],[Σ11Σ21Σ12Σ22])(27)

P ( x 1 │ x 2 = a ) = N ( μ 1 + Σ 12 Σ 22 − 1 ( a − μ 2 ) , Σ 11 − Σ 12 Σ 22 − 1 Σ 21 ) ( 28 ) P(x_1│x_2=a)= N(μ_1+Σ_{12}Σ_{22}^{-1}(a-μ_2 ),Σ_{11}-Σ_{12}Σ_{22}^{-1} Σ_{21} ) \quad(28) P(x1│x2=a)=N(μ1+Σ12Σ22−1(a−μ2),Σ11−Σ12Σ22−1Σ21)(28)

P ( X t │ Y 1 , ⋯ Y t ) = = N ( A μ ^ t − 1 + Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( Y t − H A μ ^ t − 1 ) , A Σ ^ t − 1 A T + Q − Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 H Σ ˉ t ) = N ( μ ˉ t + Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( Y t − H μ ˉ t ) , Σ ˉ t − Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 H Σ ˉ t ) ( 29 ) P(X_t│Y_1,⋯Y_t )= \\= N(A\hat{\mu}_{t-1}+\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1} (Y_t-HA\hat{\mu}_{t-1} ),A\hat{\Sigma}_{t-1} A^T+Q-\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1}H\bar{\Sigma}_t ) \\=N(\bar{\mu}_t+\bar{\Sigma}_tH^T (H\bar{\Sigma}_t H^T+R)^{-1}(Y_t-H\bar{\mu}_t ),\bar{\Sigma}_t -\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1} H\bar{\Sigma}_t ) \quad(29) P(Xt│Y1,⋯Yt)==N(Aμ^t−1+ΣˉtHT(HΣˉtHT+R)−1(Yt−HAμ^t−1),AΣ^t−1AT+Q−ΣˉtHT(HΣˉtHT+R)−1HΣˉt)=N(μˉt+ΣˉtHT(HΣˉtHT+R)−1(Yt−Hμˉt),Σˉt−ΣˉtHT(HΣˉtHT+R)−1HΣˉt)(29)

{ μ ^ t = μ ˉ t + Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( Y t − H μ ˉ t ) Σ ^ t = Σ ˉ t − Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 H Σ ˉ t ) ( 30 ) \begin{cases} \hat{\mu}_t=\bar{\mu}_t+\bar{\Sigma}_tH^T (H\bar{\Sigma}_t H^T+R)^{-1}(Y_t-H\bar{\mu}_t) \\ \hat{\Sigma}_t =\bar{\Sigma}_t -\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1} H\bar{\Sigma}_t )\\ \end{cases} \quad (30) {μ^t=μˉt+ΣˉtHT(HΣˉtHT+R)−1(Yt−Hμˉt)Σ^t=Σˉt−ΣˉtHT(HΣˉtHT+R)−1HΣˉt)(30)

化解后得，更新公式：

K t = Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( 31 ) K_t=\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1}\quad (31) Kt=ΣˉtHT(HΣˉtHT+R)−1(31)

μ ^ t = μ ˉ t + Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( Y t − H μ ˉ t ) = μ ˉ t + K t ( Y t − H μ ˉ t ) ( 32 ) \hat{\mu}_t=\bar{\mu}_t+\bar{\Sigma}_tH^T (H\bar{\Sigma}_t H^T+R)^{-1}(Y_t-H\bar{\mu}_t)\\=\bar{\mu}_t+K_t (Y_t-H\bar{\mu}_t) \quad (32) μ^t=μˉt+ΣˉtHT(HΣˉtHT+R)−1(Yt−Hμˉt)=μˉt+Kt(Yt−Hμˉt)(32)

Σ ^ t = Σ ˉ t − Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 H Σ ˉ t = Σ ˉ t − K t H Σ ˉ t ( I − K t H ) Σ ˉ t ( 33 ) \hat{\Sigma}_t =\bar{\Sigma}_t -\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1} H\bar{\Sigma}_t \\=\bar{\Sigma}_t -K_t H\bar{\Sigma}_t (I-K_t H) \bar{\Sigma}_t \quad(33) Σ^t=Σˉt−ΣˉtHT(HΣˉtHT+R)−1HΣˉt=Σˉt−KtHΣˉt(I−KtH)Σˉt(33)

总结一下，至此就推导出了kalman滤波的5个公式：

预测公式：

μ ˉ t = A μ ^ t − 1 ( 18 ) \bar{\mu}_t=A\hat{\mu}_{t-1} \quad(18) μˉt=Aμ^t−1(18)

Σ ˉ t = A Σ ^ t − 1 A T + Q ( 19 ) \bar{\Sigma}_t=A\hat{\Sigma}_{t-1} A^T+Q \quad(19) Σˉt=AΣ^t−1AT+Q(19)

更新公式：

K t = Σ ˉ t H T ( H Σ ˉ t H T + R ) − 1 ( 31 ) K_t=\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1}\quad (31) Kt=ΣˉtHT(HΣˉtHT+R)−1(31)

μ ^ t = μ ˉ t + K t ( Y t − H μ ˉ t ) ( 32 ) \hat{\mu}_t=\bar{\mu}_t+K_t (Y_t-H\bar{\mu}_t) \quad (32) μ^t=μˉt+Kt(Yt−Hμˉt)(32)

Σ ^ t = ( I − K t H ) Σ ˉ t ( 33 ) \hat{\Sigma}_t =(I-K_t H) \bar{\Sigma}_t \quad(33) Σ^t=(I−KtH)Σˉt(33)

卡尔曼滤波算法的流程图（摘自维基百科）：

图2：卡尔曼滤波算法流程图

图解卡尔曼滤波算法：

图3通过一个卡尔曼滤波的例子，直观展示了在预测和更新过程中状态变量的概率密度分布的变化情况。

图3：图解卡尔曼滤波算法

其中， x ^ t − 1 \hat{x}_{t-1} x^t−1 表示 t − 1 t-1 t−1 时刻更新的状态， x ˉ t \bar{x}_{t} xˉt 表示 t t t 时刻预测的状态， y t y_{t} yt 表示 t t t 时刻的量测， x ^ t \hat{x}_{t} x^t 表示 t t t 时刻更新的状态，图中红色的直线对应的x坐标表示真实的状态。从图3可以看出，经过卡尔曼滤波算法（预测： x ^ t − 1 \hat{x}_{t-1} x^t−1 − − > --> −−> x ˉ t \bar{x}_{t} xˉt；更新： x ^ t − 1 \hat{x}_{t-1} x^t−1， y t y_{t} yt − − > --> −−> x ^ t \hat{x}_{t} x^t ）更新过后的状态将更加接近于真实的状态。