Article Directory
-
- 3. Kalman filter
-
- 1. Stochastic system state space model
- 2. State prediction
- 3. State measurement
- 4. Gain matrix K and state estimation
- 5. Summary of Kalman filter formula
- 6. Kalman filter flow chart
- 7. Circuit diagram of Kalman filtering calculation system
- 8. Kalman programming calculation flow chart
- 9. Kalman filter characteristics
- 10. Kalman filter with deterministic input
3. Kalman filter
1. Stochastic system state space model
{ X k = Φ k / k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \left\{\begin{array}{l}\boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}\end{array}\right. { Xk=Phik/k−1Xk−1+Ck−1Wk−1Zk=HkXk+Vk
The basic assumption about noise: system noise W k \boldsymbol{W}_{k}Wkand measurement noise V k \boldsymbol{V}_{k}Vk 都是零均值高斯白噪声，且不相关；系统噪声协方差阵 Q k \boldsymbol{Q}_{k} Qk 对称非负定，观测噪声协方差阵 R k \boldsymbol{R}_{k} Rk 对称正定。
- 其实改系统噪声前面系数 Γ k \Gamma_{k} Γk 可以使 Q k \boldsymbol{Q}_{k} Qk 总正定。
- 必须是高斯白噪声，多个高斯白噪声线性组合还是高斯白噪声，零均值均匀分布噪声线性组合不一定还是零均值均匀分布噪声。
{ E [ W k ] = 0 , E [ W k W j T ] = Q k δ k j E [ V k ] = 0 , E [ V k V j T ] = R k δ k j E [ W k V j T ] = 0 Q k ≥ 0 , R k > 0 ，或 Q k > 0 , R k > 0 \begin{array}{l}\left\{\begin{array}{l}\mathrm{E}\left[\boldsymbol{W}_{k}\right]=\mathbf{0}, \quad \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{W}_{j}^{\mathrm{T}}\right]=\boldsymbol{Q}_{k} \delta_{k j} \\ \mathrm{E}\left[\boldsymbol{V}_{k}\right]=\mathbf{0}, \quad \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \delta_{k j} \\ \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\mathbf{0}\end{array}\right. \\ \boldsymbol{Q}_{k} \geq 0, \quad \boldsymbol{R}_{k}>0，或\boldsymbol{Q}_{k} > 0, \quad \boldsymbol{R}_{k}>0 \end{array} ⎩ ⎨ ⎧E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=RkδkjE[WkVjT]=0Qk≥0,Rk>0，或Qk>0,Rk>0
2、状态预测
状态一步预测：预测下一时刻最有可能的那个值，由于高斯白噪声对状态预测的一阶矩不起作用（线性最小方差估计）：
X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 \hat{\boldsymbol{X}}_{k / k-1}={\boldsymbol{\Phi}}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} X^k/k−1=Φk/k−1X^k−1
状态预测误差：真值-预测
X ~ k / k − 1 = X k − X ^ k / k − 1 = ( Φ k / k − 1 X k − 1 + Γ k − 1 W k − 1 ) − Φ k / k − 1 X ^ k − 1 = Φ k / k − 1 ( X k − 1 − X ^ k − 1 ) + Γ k − 1 W k − 1 = Φ k / k − 1 X ~ k − 1 + Γ k − 1 W k − 1 \begin{array}{r}\tilde{\boldsymbol{X}}_{k / k-1}=\boldsymbol{X}_{k}-\hat{\boldsymbol{X}}_{k / k-1}=\left(\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}\right)-\boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1} \\ \quad=\boldsymbol{\Phi}_{k / k-1}\left(\boldsymbol{X}_{k-1}-\hat{\boldsymbol{X}}_{k-1}\right)+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}=\boldsymbol{\Phi}_{k / k-1} \tilde{\boldsymbol{X}}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}\end{array} X~k/k−1=Xk−X^k/k−1=(Φk/k−1Xk−1+Γk−1Wk−1)−Φk/k−1X^k−1=Φk/k−1(Xk−1−X^k−1)+Γk−1Wk−1=Φk/k−1X~k−1+Γk−1Wk−1
状态预测方差阵：一步预测误差相乘的期望，乘出来有四项，由于两时刻噪声不相关，其中两项为0。所以预测的方差就是上一时刻的方差传递过来 Φ k / k − 1 P k − 1 Φ k / k − 1 T \boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}} Φk/k−1Pk−1Φk/k−1T，加上当前误差 Γ k − 1 Q k − 1 Γ k − 1 T \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} Γk−1Qk−1Γk−1T
P k / k − 1 = E [ X ~ k / k − 1 X ~ k / k − 1 T ] = E [ ( Φ k / k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) ( Φ k / k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) T ] = Φ k / k − 1 E [ X ~ k − 1 X ~ k − 1 T ] Φ k / k − 1 T + Γ k − 1 E [ W k − 1 W k − 1 T ] Γ k − 1 T = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T \begin{aligned} \boldsymbol{P}_{k / k-1} & =\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k / k-1} \tilde{\boldsymbol{X}}_{k / k-1}^{\mathrm{T}}\right] \\ & =\mathrm{E}\left[\left(\boldsymbol{\Phi}_{k / k-1} \tilde{\boldsymbol{X}}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}\right)\left(\boldsymbol{\Phi}_{k / k-1} \tilde{\boldsymbol{X}}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}\right)^{\mathrm{T}}\right] \\ & =\boldsymbol{\Phi}_{k / k-1} \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k-1} \tilde{\boldsymbol{X}}_{k-1}^{\mathrm{T}}\right] \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \mathrm{E}\left[\boldsymbol{W}_{k-1} \boldsymbol{W}_{k-1}^{\mathrm{T}}\right] \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ & =\boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \end{aligned} Pk/k−1=E[X~k/k−1X~k/k−1T]=E( F _k/k−1X~k−1+Ck−1Wk−1)( Fk/k−1X~k−1+Ck−1Wk−1)T]=Phik/k−1E[X~k−1X~k−1T]Phik/k−1T+Ck−1E[Wk−1Wk−1T]Ck−1T=Phik/k−1Pk−1Phik/k−1T+Ck−1Qk−1Ck−1T
3. State measurement
Measurement : The measurement value is brought into the measurement equation, and the influence of Gaussian white noise is also ignored:
Z ^ k / k − 1 = H k X ^ k / k − 1 \hat{\boldsymbol{Z}}_{k / k-1}=\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}Z^k/k−1=HkX^k/k−1
量测误差：
Z ~ k / k − 1 = Z k − Z ^ k / k − 1 = ( H k X k + V k ) − H k X ^ k / k − 1 = H k X ~ k / k − 1 + V k \begin{aligned} \tilde{\boldsymbol{Z}}_{k / k-1} & =\boldsymbol{Z}_{k}-\hat{\boldsymbol{Z}}_{k / k-1}=\left(\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}\right)-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1} =\boldsymbol{H}_{k} \tilde{\boldsymbol{X}}_{k / k-1}+\boldsymbol{V}_{k}\end{aligned} Z~k/k−1=Zk−Z^k/k−1=(HkXk+Vk)−HkX^k/k−1=HkX~k/k−1+Vk
Measure variance matrix :
P Z Z , k / k − 1 = E [ Z ~ k / k − 1 Z ~ k / k − 1 T ] = E [ ( H k X ~ k / k − 1 + V k ) ( H k X ~ k / k − 1 + V k ) T ] = H k E [ X ~ k / k − 1 X ~ k / k − 1 T ] H k T + E [ V k V k T ] = H k P k / k − 1 H k T + R k \begin{aligned} \boldsymbol{P}_{Z Z, k / k-1} & =\mathrm{E}\left[\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}\right] \\ & =\mathrm{E}\left[\left(\boldsymbol{H}_{k} \tilde{\boldsymbol{X}}_{k / k-1}+\boldsymbol{V}_{k}\right)\left(\boldsymbol{H}_{k} \tilde{\boldsymbol{X}}_{k / k-1}+\boldsymbol{V}_{k}\right)^{\mathrm{T}}\right] \\ & =\boldsymbol{H}_{k} \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k / k-1} \tilde{\boldsymbol{X}}_{k / k-1}^{\mathrm{T}}\right] \boldsymbol{H}_{k}^{\mathrm{T}}+\mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{k}^{\mathrm{T}}\right] \\ & =\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\end{aligned} PZZ,k/k−1=E[Z~k/k−1Z~k/k−1T]=E[(HkX~k/k−1+Vk)(HkX~k/k−1+Vk)T]=HkE[X~k/k−1X~k/k−1T]HkT+E[VkVkT]=HkPk/k−1HkT+Rk
量测互协方差：X 与 Z 之间，状态的一步预测误差和量测之间的互协方差
一般不为0，为0就没有滤波的必要了，量测值直接就是估计值
P X Z , k / k − 1 = E [ X ~ k / k − 1 Z ~ k / k − 1 T ] = E [ X ~ k / k − 1 ( H k X ~ k / k − 1 + V k ) T ] = P k / k − 1 H k T \begin{array}{l}\boldsymbol{P}_{X Z, k / k-1}=\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}\right] \\ \quad=\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k / k-1}\left(\boldsymbol{H}_{k} \tilde{\boldsymbol{X}}_{k / k-1}+\boldsymbol{V}_{k}\right)^{\mathrm{T}}\right] \\ \quad=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\end{array} PXZ , k / k − 1=E[X~k/k−1Z~k/k−1T]=E[X~k/k−1(HkX~k/k−1+Vk)T]=Pk/k−1HkT
4、增益矩阵K与状态估计
假设滤波方程可以写成 状态预测+K量测修正 形式， K {\color{red}K} K 矩阵先待定
状态估计：
X ^ k = X ^ k / k − 1 + K k Z k ~ = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k / k-1}+{\color{red}\boldsymbol{K}_{k} }\tilde{
{\boldsymbol{Z}}_{k}}=\hat{\boldsymbol{X}}_{k / k-1}+{\color{red}\boldsymbol{K}_{k}}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) X^k=X^k/k−1+KkZk~=X^k/k−1+Kk(Zk−HkX^k/k−1)
状态估计误差：
X ~ k = X k − X ^ k = X k − [ X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) ] = X ~ k / k − 1 − K k ( H k X k + V k − H k X ^ k / k − 1 ) = ( I − K k H k ) X ~ k / k − 1 − K k V k \begin{aligned} \tilde{\boldsymbol{X}}_{k} & =\boldsymbol{X}_{k}-\hat{\boldsymbol{X}}_{k}=\boldsymbol{X}_{k}-\left[\hat{\boldsymbol{X}}_{k / k-1}+{\color{red}\boldsymbol{K}_{k}}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)\right] \\ & =\tilde{\boldsymbol{X}}_{k / k-1}-{\color{red}\boldsymbol{K}_{k}}\left(\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \tilde{\boldsymbol{X}}_{k / k-1}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{V}_{k}\end{aligned} X~k=Xk−X^k=Xk−[X^k/k−1+Kk(Zk−HkX^k/k−1)]=X~k/k−1−Kk(HkXk+Vk−HkX^k/k−1)=(I−KkHk)X~k/k−1−KkVk
State estimation variance matrix :
P k = E [ X ~ k X ~ k T ] = E { [ ( I − K k H k ) X ~ k / k − 1 − K k V k ] [ ( I − K k H k ) X ~ k / k − 1 − K k V k ] T } = ( I − K k H k ) E [ X ~ k / k − 1 X ~ k / k − 1 T ] ( I − K k H k ) T + K k E [ V k V k T ] K k T = ( I − K k H k ) P k / k − 1 ( I − K k H k ) T + K k R k K k T \begin{aligned} \boldsymbol{P}_{k} &= \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right] \\ & =\mathrm{E}\left\{\left[\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{H}_{k}\right) \tilde{\boldsymbol{X}}_{k / k-1}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{V}_{k}\right]\left[\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \tilde{\boldsymbol{X}}_{k / k-1}-{\color{red}\boldsymbol{K}_{k} }\boldsymbol{V}_{k}\right]^{\mathrm{T}}\right\} \\ & =\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}_{k}}\boldsymbol{H}_{k}\right) \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k / k-1} \tilde{\boldsymbol{X}}_{k / k-1}^{\mathrm{T}}\right]\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{H}_{k}\right)^{\mathrm{T}}+{\color{red}\boldsymbol{K}_{k}} \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{k}^{\mathrm{T}}\right] {\color{red}\boldsymbol{K}_{k}^{\mathrm{T}}} \\ & =\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}\left(\boldsymbol{I}-{\color{red}\boldsymbol{K}_{k}} \boldsymbol{H}_{k}\right)^{\mathrm{T}}+{\color{red}\boldsymbol{K}_{k}} \boldsymbol{R}_{k} {\color{red}\boldsymbol{K}_{k}^{\mathrm{T}}}\end{aligned} Pk=E[X~kX~kT]=E{
[(I−KkHk)X~k/k−1−KkVk][(I−KkHk)X~k/k−1−KkVk]T}=(I−KkHk)E[X~k/k−1X~k/k−1T](I−KkHk)T+KkE[VkVkT]KkT=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT
到此，都是假设，在待定 K {\color{red}K} K 的情况下进行状态预测，并算出方差阵，下面就是要求 K {\color{red}K} K ：
K {\color{red}K} K 矩阵可以取某个值，使状态估计方差 P k \boldsymbol{P}_{k} Pk 达到最小，且保持 P k \boldsymbol{P}_{k} Pk 对称正定。
要理解矩阵最小的含义，得先理解 A A A 矩阵小于 B B B 矩阵的含义： B − A B-A B−A 正定
The meaning of optimal estimation : the second-order moment
of each component reaches the minimum : E [ ( X ~ k ( 1 ) ) 2 ] + E [ ( X ~ k ( 2 ) ) 2 ] + ⋯ + E [ ( X ~ k ( n ) ) 2 ] = min \mathrm{E}\left[\left(\tilde{X}{k}{(1)}\right)^{2}\right]+\mathrm{E} \left[\left(\tilde{X}{k}^{(2)}\right)^{2}\right]+\cdots+\mathrm{E}\left[\left(\tilde{X}_ {k}{(n)}\right)^{2}\right]=\min
E[(X~k(1))2]+E[(X~k(2))2]+⋯+E[(X~k(n))2]=min
即： E [ X ~ k T X ~ k ] = min \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k}^{\mathrm{T}} \tilde{\boldsymbol{X}}_{k}\right]=\min E[X~kTX~k]=min：
E [ X ~ k X ~ k T ] = [ E [ ( X k ( 1 ) ) 2 ] E [ X k ( 1 ) X k ( 2 ) ] ⋯ E [ X k ( 1 ) X k ( n ) ] E [ X ~ k ( 2 ) X ~ k ( 1 ) ] E [ ( X ~ k ( 2 ) ) 2 ] ⋯ E [ X ~ k ( 2 ) X ~ k ( n ) ] ⋮ ⋮ ⋱ ⋮ E [ X ~ k ( n ) X ~ k ( 1 ) ] E [ X ~ k ( n ) X ~ k ( 2 ) ] ⋯ E [ ( X ~ k ( n ) ) 2 ] ] \mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]=\left[\begin{array}{cccc}\mathrm{E}\left[\left(X_{k}^{(1)}\right)^{2}\right] & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[X_{k}^{(1)} X_{k}^{(n)}\right] \\ \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\left(\tilde{X}_{k}^{(2)}\right)^{2}\right] & \cdots & \mathrm{E}\left[\tilde{X}_{k}^{(2)} \tilde{X}_{k}^{(n)}\right] \\ \vdots & \vdots & \ddots & \vdots \\ \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(1)}\right] & \mathrm{E}\left[\tilde{X}_{k}^{(n)} \tilde{X}_{k}^{(2)}\right] & \cdots & \mathrm{E}\left[\left(\tilde{X}_{k}^{(n)}\right)^{2}\right]\end{array}\right] E[X~kX~kT]= E[(Xk(1))2]E[X~k(2)X~k(1)]⋮E[X~k(n)X~k(1)]E[Xk(1)Xk(2)]E[(X~k(2))2]⋮E[X~k(n)X~k(2)]⋯⋯⋱⋯E[Xk(1)Xk(n)]E[X~k(2)X~k(n)]⋮E[(X~k(n))2]
只需要关注对角线上的元素，即求迹：
tr ( P k ) = tr ( E [ X ~ k X ~ k T ] ) = min \operatorname{tr}\left(\boldsymbol{P}_{k}\right)=\operatorname{tr}\left(\mathrm{E}\left[\tilde{\boldsymbol{X}}_{k} \tilde{\boldsymbol{X}}_{k}^{\mathrm{T}}\right]\right)=\min tr(Pk)=tr(E[X~kX~kT])=min
由最优估计的含义可知，要得到最优估计等价于求 P k \boldsymbol{P}_{k} Pk 的迹，最小，先展开：
P k = ( I − K k H k ) P k / k − 1 ( I − K k H k ) T + K k R k K k T = P k / k − 1 − K k H k P k / k − 1 − ( K k H k P k / k − 1 ) T + K k ( H k P k / k − 1 H k T + R k ) K k T \begin{aligned} \boldsymbol{P}_{k} & =\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right)^{\mathrm{T}}+\boldsymbol{K}_{k} \boldsymbol{R}_{k} \boldsymbol{K}_{k}^{\mathrm{T}} \\ & =\boldsymbol{P}_{k / k-1}-\boldsymbol{K}_{k} \boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}-\left(\boldsymbol{K}_{k} \boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}\right)^{\mathrm{T}}+\boldsymbol{K}_{k}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right) \boldsymbol{K}_{k}^{\mathrm{T}}\end{aligned} Pk=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT=Pk/k−1−KkHkPk/k−1−(KkHkPk/k−1)T+Kk(HkPk/k−1HkT+Rk)KkT
Then find the trace and decompose it, which is a multi-input and single-output multivariate function:
tr ( P k ) = tr ( P k / k − 1 ) − tr ( K k H k P k / k − 1 ) − tr ( ( K k H k P k / k − 1 ) T ) + tr ( K k ( H k P k / k − 1 H k T + R k ) K k T ) \begin{aligned} \ operatorname{tr}\left(\boldsymbol{P}_{k}\right)= & \operatorname{tr}\left(\boldsymbol{P}_{k / k-1}\right)-\operatorname{tr }\left(\boldsymbol{K}_{k} \boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}\right) \\ & -\operatorname{tr}\left( \left(\boldsymbol{K}_{k} \boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}\right)^{\mathrm{T}}\right)+\ operatorname{tr}\left(\boldsymbol{K}_{k}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k} ^{\mathrm{T}}+\boldsymbol{R}_{k}\right) \boldsymbol{K}_{k}^{\mathrm{T}}\right)\end{aligned}tr(Pk)=tr(Pk/k−1)−tr(KkHkPk/k−1)−tr((KkHkPk/k−1)T)+tr(Kk(HkPk/k−1HkT+Rk)KkT)
To require the extremum of the multivariate function is to find the derivative equal to 0 00 , according to the derivation rule:
dd X [ tr ( XB ) ] = dd X [ tr ( ( XB ) T ) ] = BT dd X [ tr ( XAXT ) ] = 2 XA , A is a symmetric matrix When \begin{aligned}\frac{\mathrm{d}}{\mathrm{d} \boldsymbol{X}}[\operatorname{tr}(\boldsymbol{X} \boldsymbol{B})]=\frac{ \mathrm{d}}{\mathrm{d} \boldsymbol{X}}\left[\operatorname{tr}\left((\boldsymbol{X} \boldsymbol{B})^{\mathrm{T}}\ right)\right]=\boldsymbol{B}^{\mathrm{T}} \\ \frac{\mathrm{d}}{\mathrm{d} \boldsymbol{X}}\left[\operatorname{tr} \left(\boldsymbol{X} \boldsymbol{A} \boldsymbol{X}^{\mathrm{T}}\right)\right]=2 \boldsymbol{X} \boldsymbol{A}, when A is a symmetrical matrix \end{aligned}dXd[tr(XB)]=dXd[tr((XB)T)]=BTdXd[tr( X A XT)]=2 X A , when A is a symmetrical matrix
求导得：
d d K k [ tr ( P k ) ] = 0 − ( H k P k / k − 1 ) T − ( H k P k / k − 1 ) T + 2 K k ( H k P k / k − 1 H k T + R k ) = 0 \frac{\mathrm{d}}{\mathrm{d} \boldsymbol{K}_{k}}\left[\operatorname{tr}\left(\boldsymbol{P}_{k}\right)\right]=\mathbf{0}{\color{green}-\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}\right)^{\mathrm{T}}-\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1}\right)^{\mathrm{T}}}+2 {\color{red}\boldsymbol{K}_{k}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)=\mathbf{0} dKkd[tr(Pk)]=0−(HkPk/k−1)T−(HkPk/k−1)T+2Kk(HkPk/k−1HkT+Rk)=0
绿色的部分为移到左边，两边消去 2 得：
P k / k − 1 H k T = K k ( H k P k / k − 1 H k T + R k ) \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}={\color{red}\boldsymbol{K}_{k}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right) Pk/k−1HkT=Kk(HkPk/k−1HkT+Rk)
要求出 K {\color{red}K} K ，需要 K {\color{red}K} K 后面部分可逆，即量测噪声 R k \boldsymbol{R}_{k} RkTo be positive; the previous H k P k / k − 1 H k \boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}HkPk/k−1HkSince H k \boldsymbol{H}_{k}HkThere are many 0 00 element, reversible will not be positive definite, if adding one is always positivedefinite R k \boldsymbol{R}_{k}Rk, it can also maintain the positive concentration of the whole, and continue to seek inversion.
K {\color{red}K} The latter part of K can be multiplied inversely to both sides:
K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 {\color{red}\boldsymbol{K}_ {k}}=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{ P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1}Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)− 1
will find the gain matrixKKK is brought back to the equation of the uppermost state estimation and variance, and the Kalman filter solution is obtained.
By eliminating some elements, a shorter variance can be obtained: P k = ( I − K k H k ) P k / k − 1 \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{ K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}Pk=(I−KkHk)Pk/k−1
5. Summary of Kalman filter formula
-
State one-step prediction: X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 \hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{\Phi}_{k /k-1} \hat{\boldsymbol{X}}_{k-1}X^k/k−1=Phik/k−1X^k−1
-
State one-step forecast mean square error: P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T \boldsymbol{P} _{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{ \mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}Pk/k−1=Phik/k−1Pk−1Phik/k−1T+Ck−1Qk−1Ck−1T
-
Filter gain: K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 \boldsymbol{K}_{k}=\boldsymbol{P}_{ k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol {H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1}Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1
-
State estimation: X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol {X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{ X}}_{k / k-1}\right)X^k=X^k/k−1+Kk(Zk−HkX^k/k−1)
-
State estimation mean square error: P k = ( I − K k H k ) P k / k − 1 \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k } \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}Pk=(I−KkHk)Pk/k−1
6. Kalman filter flow chart
1. Divided into left and right parts (first-order moment and second-order moment)
- The filter calculation loop on the left is the update process of the first moment
- The gain calculation loop on the right is the update process of the second moment
- The left has no effect on the right, and the right has an effect on the left through the gain matrix. Sometimes the value is not good, and the calculation on the left side collapses, and it has no effect on the right side. It is still calculated according to the model, and the calculated variance value is still very small, which cannot reflect the real error situation.
2. Divided into upper and lower parts (time update, measurement update)
- The above is the time update, which is only related to the state equation.
- The following is the measurement update, which is only related to the measurement equation.
7. Circuit diagram of Kalman filtering calculation system
- In general, it is a linear system, input Z k Z_{k}Zk, output X ^ k \hat{ {X}}_{k}X^k
- The right part is the time update, which takes the output of the system at the previous moment as input and outputs the predicted state at the current moment
- The left part is the measurement update
- Kalman filtering is a minimum variance linear unbiased estimate for linear systems
8. Kalman programming calculation flow chart
- If it is not a steady system, Φ k / k − 1 , Γ k − 1 \boldsymbol{\Phi}_{k / k-1}, \boldsymbol{\Gamma}_{k-1}Phik/k−1,Ck−1, Q k − 1 \boldsymbol{Q}_{k-1} Qk−1, H k , R k \boldsymbol{H}_{k}, \boldsymbol{R}_{k} Hk,RkThey are all time-varying and need to be given; the steady system can be used as a constant.
- If the system is nonlinear and is a high-order system, the initial state X ^ 0 , P 0 \hat{\boldsymbol{X}}_{0} ,\boldsymbol{P}_{0} must be givenX^0,P0
- Output first-order moment, second-order moment
- The first-order moment and second-order moment can represent the entire system, and the random signal for Kalman filtering needs to be normally distributed
- The second moment is to calculate the weight for the next moment (gain matrix KKK）
- Measurement updates and time updates may be out of sync; common is measurement update frequency less than time update frequency
9. Kalman filter characteristics
1. Linear combination of measurements in state estimation
Kalman filtering is a recursive formula, and the previous moment can be recursive to the current moment, and the recursion continues. Expressed in terms of forecast and measure weights:
X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) = ( I − K k H k ) Φ k / k − 1 X ^ k − 1 + K k Z k = G k X ^ k − 1 + K k Z k = G k ( G k − 1 X ^ k − 2 + K k − 1 Z k − 1 ) + K k Z k = G k G k − 1 X ^ k − 2 + G k K k − 1 Z k − 1 + K k Z k = G k G k − 1 G k − 2 X ^ k − 3 + G k G k − 1 K k − 2 Z k − 2 + G k K k − 1 Z k − 1 + K k Z k = ⋯ = ( ∏ i = 1 k G i ) X ^ 0 + ∑ i = 1 k ( ∏ j = i + 1 k G j ) K i Z i \begin{aligned} \hat{\boldsymbol{X}}_{k} & =\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) \\ & =\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{K}_{k} \boldsymbol{Z}_{k}=\boldsymbol{G}_{k} \hat{\boldsymbol{X}}_{k-1}+\boldsymbol{K}_{k} \boldsymbol{Z}_{k} \\ & =\boldsymbol{G}_{k}\left(\boldsymbol{G}_{k-1} \hat{\boldsymbol{X}}_{k-2}+\boldsymbol{K}_{k-1} \boldsymbol{Z}_{k-1}\right)+\boldsymbol{K}_{k} \boldsymbol{Z}_{k} \\ & =\boldsymbol{G}_{k} \boldsymbol{G}_{k-1} \hat{\boldsymbol{X}}_{k-2}+\boldsymbol{G}_{k} \boldsymbol{K}_{k-1} \boldsymbol{Z}_{k-1}+\boldsymbol{K}_{k} \boldsymbol{Z}_{k} \\ & =\boldsymbol{G}_{k} \boldsymbol{G}_{k-1} \boldsymbol{G}_{k-2} \hat{\boldsymbol{X}}_{k-3}+\boldsymbol{G}_{k} \boldsymbol{G}_{k-1} \boldsymbol{K}_{k-2} \boldsymbol{Z}_{k-2}+\boldsymbol{G}_{k} \boldsymbol{K}_{k-1} \boldsymbol{Z}_{k-1}+\boldsymbol{K}_{k} \boldsymbol{Z}_{k} \\ & =\cdots \\ & =\left(\prod_{i=1}^{k} \boldsymbol{G}_{i}\right) \hat{\boldsymbol{X}}_{0}+\sum_{i=1}^{k}\left(\prod_{j=i+1}^{k} \boldsymbol{G}_{j}\right) \boldsymbol{K}_{i} \boldsymbol{Z}_{i}\end{aligned} X^k=X^k/k−1+Kk(Zk−HkX^k/k−1)=(I−KkHk)Phik/k−1X^k−1+KkZk=GkX^k−1+KkZk=Gk(Gk−1X^k−2+Kk−1Zk−1)+KkZk=GkGk−1X^k−2+GkKk−1Zk−1+KkZk=GkGk−1Gk−2X^k−3+GkGk−1Kk−2Zk−2+GkKk−1Zk−1+KkZk=⋯=(i=1∏kGi)X^0+i=1∑k(j=i+1∏kGj)KiZi
If the initial value X ^ 0 \hat{\boldsymbol{X}}_{0}X^0Value 0 00 , the current estimated value is all previous measured valuesZ i \boldsymbol{Z_i}Zilinear combination of
2. Orthogonal projection properties
- Kalman filter estimation is the weighted average of prediction and measurement.
- The measurement is a vector, and the prediction is also a vector, which is multiplied by a weighting coefficient and added together.
- No matter what the previous coefficients are, the estimated values obtained by adding them are all on the same plane.
- The real state is also a vector in space.
- To minimize the direct error between the estimated value and the true value, that is, to take the point on the plane with the smallest distance to a point outside the plane, it is to take the orthogonal projection point.
3. The innovation vector is a zero-matrix white noise sequence
{ E [ Z ~ k / k − 1 ] = 0 E [ Z ~ k / k − 1 Z ~ j / j − 1 T ] = ( H k P k / k − 1 H k T + R k ) δ k j \left\{\begin{array}{l}\mathrm{E}\left[\tilde{\boldsymbol{Z}}_{k / k-1}\right]=\mathbf{0} \\ \mathrm{E}\left[\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{j / j-1}^{\mathrm{T}}\right]=\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right) \delta_{k j}\end{array}\right. ⎩ ⎨ ⎧E[Z~k/k−1]=0E[Z~k/k−1Z~j/j−1T]=(HkPk/k−1HkT+Rk)δkj
10、带确定性输入的Kalman滤波
状态空间模型：
{ X k = Φ k / k − 1 X k − 1 + B k − 1 u k − 1 + Γ k − 1 W k − 1 Z k = H k X k + Y k + V k \left\{\begin{array}{l}\boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+{\color{red}\boldsymbol{B}_{k-1} \boldsymbol{u}_{k-1}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+{\color{red}\boldsymbol{Y}_{k}}+\boldsymbol{V}_{k}\end{array}\right. {
Xk=Phik/k−1Xk−1+Bk−1uk−1+Ck−1Wk−1Zk=HkXk+Yk+Vk
Filtering equation, only the part related to the first moment needs to be changed:
{ X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 + B k − 1 u k − 1 P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 X ^ k = X ^ k / k − 1 + K k ( Z k − Y k − H k X ^ k / k − 1 ) P k = ( I − K k H k ) P k / k − 1 \left\{\begin{array}{l}\hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \hat{\boldsymbol{X}}_{k-1}+{\color{red}\boldsymbol{B}_{k-1} \boldsymbol{u}_{k-1}} \\ \boldsymbol{P}_{k / k-1}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ \boldsymbol{K}_{k}=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\hat{\boldsymbol{X}}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-{\color{red}\boldsymbol{Y}_{k}}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) \\ \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}\end{array}\right. ⎩
⎨
⎧X^k/k−1=Φk/k−1X^k−1+Bk−1uk−1Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1X^k=X^k/k−1+Kk(Zk−Yk−HkX^k/k−1)Pk=(I−KkHk)Pk/k−1