卡尔曼滤波五个公式

\(P_{[k]} \\ =E(e_{[k]}e_{[k]}^{\mathrm{T}})\)
\(=E(((I-K_{[k]}H_{{m}})e_{[k]}^{-}-K_{[k]}v_{[k]}) ((I-K_{[k]}H_{\mathrm{m}})e_{[k]}^{-}-K_{[k]}v_{[k]})^T)\)

% 第二部分内容转置到括号里面
\(= E(((I-K_{[k]}H_{m})e_{[k]}^{-}-K_{[k]}v_{[k]})(e_{[k]}^{-}{}^{\mathrm{T}}(I-K_{[k]}H_{m})^{\mathrm{T}}-v_{[k]}^{\mathrm{T}}K_{[k]}^{\mathrm{T}}))\)

% 展开成四项
\(=E((I-K_{[k]}H_{m})e_{[k]}^{-}e_{[k]}^{-T}(I-K_{[k]}H_{m})^{\mathrm{T}}\)
\(\quad -(I-K_{[k]}H_{m})e_{[k]}^{-} v_{[k]}^{\mathrm{T}}K_{[k]}^{\mathrm{T}}\)
\(\quad -K_{[k]} v_{[k]} e_{[k]}^{-T} (I-K_{[k]}H_{m})^T\)
\(\quad +K_{[k]} v_{[k]} v_{[k]}^{\mathrm{T}}K_{[k]}^{\mathrm{T}})\)

%期望运算
\(=E((I-K_{[k]}H_{m})e_{[k]}^{-}e_{[k]}^{-T} (I-K_{[k]}H_{m})^T)\)
\(\quad -E((I-K_{[k]}H_{m})e_{[k]}^{-}v_{[k]}^{\mathrm{T}}K_{[k]}^{T})\)
\(\quad -E(K_{[k]} v_{[k]}e_{[k]}^{-T}(I-K_{[k]}H_{m})^{\mathrm{T}})\)
\(\quad E(K_{[k]} v_{[k]} v_{[k]}^{\mathrm{T}}K_{[k]}^{\mathrm{T}})\)

% I为单位矩阵，K和H是常数矩阵
\(= (I-K_{[k]}H_{m})E(e_{[k]}^{-}e_{[k]}^{-T})(I-K_{[k]}H_{m})^{\mathrm{T}}\)
\(\quad +(I-K_{[k]}H_{m})E(e_{[k]}^{-}v_{[k]}^{\mathrm{T}})K_{[k]}^{\mathrm{T}}\)
\(\quad -K_{[k]}E(v_{[k]}e_{[k]}^{-\mathrm{T}})\)
\(\quad +(I-K_{[k]}H_{\mathrm{m}})^{\mathrm{T}}+K_{[k]}E(v_{[k]}v_{[k]}^{\mathrm{T}})K_{[k]}^T\)

% 因为测量误差 $ v_{[k]}$ 和先验状态估计误差 \(e_{k}^-\) 相互独立，而且 \(E(v_{[k]})=E(v_{[k]}^{\mathrm{T}})=0\) 因此中间两项为0

\(P_{[k]} =\\ \quad (I-K_{[k]}H_{m})E(e_{[k]}^{-}e_{[k]}^{-T})(I-K_{[k]}H_{m})^{\mathrm{T}} +\)

\(\quad +K_{[k]}E(v_{[k]}v_{[k]}^{\mathrm{T}})K_{[k]}^T\)

% 代入先验估计误差矩阵，和 测量误差矩阵

$ P_{[k]} =
(I-K_{[k]}H_{m})P_{[k]}^- (I-K_{[k]}H_{m})^{\mathrm{T}}
+K_{[k]}R_cK_{[k]}^T $

$\quad \quad =(P_{[k]}^- -K_{{k}} H_m P_{[k]}^- ) (I-H_{m}^T K_{[k]}^T)
+K_{[k]}R_cK_{[k]}^T $

\(\quad\quad=P_{[k]}^{-}-P_{[k]}^{-}H_{m}^T K_{[k]}^T-K_{[k]}H_{m}P_{[k]}^{-}+\\ \quad\quad\quad K_{[k]}H_{m}P_{[k]}^{-}H_{m}^T K_{[k]}^{\mathrm{T}}+K_{[k]}R_{c}K_{[k]}^T\)

后验状态估计协方差矩阵的迹为

\(\begin{aligned} \operatorname{Tr}\left(\boldsymbol{P}_{[k]}\right)= & \operatorname{Tr}\left(\boldsymbol{P}_{[k]}^{-}\right)-\operatorname{Tr}\left(\boldsymbol{P}_{[k]}^{-} \boldsymbol{H}_{\mathrm{m}}^{\mathrm{T}} \boldsymbol{K}_{[k]}^{\mathrm{T}}\right)-\operatorname{Tr}\left(\boldsymbol{K}_{[k]} \boldsymbol{H}_{\mathrm{m}} \boldsymbol{P}_{[k]}^{-}\right)+ \\ & \operatorname{Tr}\left(\boldsymbol{K}_{[k]} \boldsymbol{H}_{\mathrm{m}} \boldsymbol{P}_{[k]}^{-} \boldsymbol{H}_{\mathrm{m}}^{\mathrm{T}} \boldsymbol{K}_{[k]}^{\mathrm{T}}\right)+\operatorname{Tr}\left(\boldsymbol{K}_{[k]} \boldsymbol{R}_{\mathrm{c}} \boldsymbol{K}_{[k]}^{\mathrm{T}}\right) \end{aligned}\)

因为协方差矩阵式对称矩阵，所以 \(P^-_{[k]} H_m^T K_{[k]}^T= (K_{[K]}H_m P^-_{[k]})^T\) 因此

\[Tr(P^-_{[k]} H_m^T K_{[k]}^T)= Tr((K_{[K]}H_m P^-_{[k]})^T) \]

代入后验状态估计协方差矩阵的迹为

\[\begin{aligned}\mathrm{Tr}(P_{[k]})&=\mathrm{Tr}(P_{[k]}^{-})-2\mathrm{Tr}(K_{[k]}H_{\mathrm{m}}P_{[k]}^{-})+\\&\mathrm{Tr}(K_{[k]}H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}K_{[k]}^{\mathrm{T}})+\mathrm{Tr}(K_{[k]}R_{\mathrm{c}}K_{[k]}^{\mathrm{T}})\end{aligned} \]

求其最小值

\[\frac{\partial\mathrm{Tr}(P_{[k]})}{\partial K_{[k]}}=0 \]

迹是标量方程，对其进行求导

\[\begin{aligned} \frac{\partial\mathrm{Tr}(P_{[k]})}{\partial K_{[k]}}& =\frac{\partial\operatorname{Tr}(P_{[k]}^{-})}{\partial K_{[k]}}-2 \frac{\partial\operatorname{Tr}(K_{[k]}H_{m}P_{[k]}^{-})}{\partial K_{[k]}}+ \\ &\frac{\partial\mathrm{Tr}(K_{[k]}H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}K_{[k]}^{\mathrm{T}})}{\partial K_{[k]}}+\frac{\partial\mathrm{Tr}(K_{[k]}R_{\mathrm{c}}K_{[k]}^{\mathrm{T}})}{\partial K_{[k]}} \\ &\text{.} =0-2(H_{\mathrm{m}}P_{[k]}^{-})^{\mathrm{T}}+2K_{[k]}H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+2K_{[k]}R_{\mathrm{c}} \\ &=-2P_{[k]}^{-}{}^{\mathrm{T}}H_{\mathrm{m}}^{\mathrm{T}}+2K_{[k]}H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+2K_{[k]}R_{\mathrm{c}} \end{aligned}\]

因为\(P^-_{[k]}\) 为对称矩阵，所以 \(P^-_{[k]}=P^{-T}_{[k]}\) 代入可得

\[\frac{\partial\mathrm{Tr}(P_{[k]})}{\partial K_{[k]}}=-2P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+2K_{[k]}(H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}}) \]

二阶导数为

\[\frac{\partial^2\operatorname{Tr}(P_{[k]})}{\partial K_{[k]}^2}=2(H_{\mathfrak{m}}P_{[k]}^-H_{\mathfrak{m}}^{\mathfrak{T}}+R_{\mathfrak{c}}) \]

它正定矩阵，因此令其等于0，可得到 最优增益

\[\begin{aligned}&-P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+K_{[k]}(H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}})=0\\&\Rightarrow K_{[k]}=\frac{P_{[k]}^{-}H_{m}^{\mathrm{T}}}{H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}}}\end{aligned} \]

上式中矩阵 \(H_m\) 和 \(R_c\) 都当成已知量，至此，只需得到先验状态估计的协方差矩阵 \(P^-_{[k]}\), 就可以得卡尔曼滤波的增益。

把

\[e_{[k]}^{-}=x_{[k]}-\hat{x}_{[k]}^{-} =Ae_{[k-1]}+w_{[k-1]} \]

代入

\[P_{[k]}^{-}=E(e_{[k]}^{-}e_{[k]}^{-T}) \]

得到

\[\begin{aligned}\mathbf{P}_{[k]}& = E(e_{[k]}^{-} e_{[k]}^{-T})= E((Ae_{[k-1]}+w_{[k-1]})(Ae_{[k-1]}+w_{[k-1]})^{T}) \\ &=E((Ae_{[k-1]}+w_{[k-1]})(e_{[k-1]}^\mathrm{T}A^\mathrm{T}+w_{[k-1]}^\mathrm{T})) \\ &=E(Ae_{[k-1]}e_{[k-1]}^{\mathrm{T}}A^{\mathrm{T}}+Ae_{[k-1]}w_{[k-1]}^{\mathrm{T}}+w_{[k-1]}e_{[k-1]}^{\mathrm{T}}A^{\mathrm{T}}+w_{[k-1]}w_{[k-1]}^{\mathrm{T}}) \\ &=AE(e_{[k-1]}e_{[k-1]}^{\mathrm{T}})A^{\mathrm{T}}+AE(e_{[k-1]}w_{[k-1]}^{\mathrm{T}})+ \\ & \quad E(w_{[k-1]}e_{[k-1]}^\mathrm{T})A^\mathrm{T}+E(w_{[k-1]}w_{[k-1]}^\mathrm{T})& \end{aligned}\]

因为 $ e_{[k-1]}$ 和 $ w_{[k]}$ 相互独立，而且 \(E\left(w_{\left[k-1\right]}^{\mathrm{T}}\right)=E\left(w_{\left[k-1\right]}\right)=0\)

上式变成

\[P_{[k]}^{-}=AE(e_{[k-1]}e_{[k-1]}^{\mathrm{T}})A^{\mathrm{T}}+E(w_{[k-1]}w_{[k-1]}^{\mathrm{T}}) \]

因此\(K_{[k]}=\frac{P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}}{H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}}}\)中的每个量都变成已知了

最后，将\(K_{[k]}=\frac{P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}}{H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}}}\) 代入

\(\begin{aligned} P_{[k]}& =(I-K_{[k]}H_{\mathrm{m}})E(e_{[k]}^{-}e_{[k]}^{-T})(I-K_{[k]}H_{\mathrm{m}})^{\mathrm{T}}- \\ &(I-K_{[k]}H_{m})E(e_{[k]}^{-}v_{[k]}^{\mathrm{T}})K_{[k]}^{\mathrm{T}}-K_{[k]}E(v_{[k]}e_{[k]}^{-\mathrm{T}}) \\ &(I-K_{[k]}H_{\mathfrak{m}})^{\mathrm{T}}+K_{[k]}E(v_{[k]} v_{[k]}^{\mathrm{T}})K_{[k]}^{\mathrm{T}} \end{aligned}\)

得到

\[\begin{aligned} \boldsymbol{P}_{[k]}& =P_{[k]}^{-}-P_{[k]}^{-}H_{m}^{\mathrm{T}}K_{[k]}^{\mathrm{T}}-K_{[k]}H_{m}P_{[k]}^{-}+K_{[k]}H_{m}P_{[k]}^{-}H^{\mathrm{T}}K_{[k]}^{\mathrm{T}}+K_{[k]}R_{\mathrm{c}}K_{[k]}^{\mathrm{T}} \\ &=P_{[k]}^{-}-P_{[k]}^{-}H_{m}^{\mathrm{T}}K_{[k]}^{\mathrm{T}}-K_{[k]}H_{m}P_{[k]}^{-}+\frac{P_{[k]}^{-}H_{m}^{\mathrm{T}}}{H_{m}P_{[k]}^{-}H_{m}^{\mathrm{T}}+R_{c}}(H_{m}P_{[k]}^{-}H_{m}^{\mathrm{T}}+R_{c})K_{[k]}^{\mathrm{T}} \\ &=P_{[k]}^{-}-P_{[k]}^{-}H_{m}^{\mathrm{T}}K_{[k]}^{\mathrm{T}}-K_{[k]}H_{m}P_{[k]}^{-}+P_{[k]}^{-}H_{m}^{\mathrm{T}}K_{[k]}^{\mathrm{T}} \\ &=P_{[k]}^{-}-K_{[k]}H_{\mathrm{m}}P_{[k]}^{-} \\ &=(I-K_{[k]}H_{\mathrm{m}})P_{[k]}^{-}& \text{(6.3.21)} \end{aligned}\]

从此得到卡尔曼滤波的五个公式：

\[\hat{x}_{[k]}^{-}=A\hat{x}_{[k-1]}+Bu_{[k-1]} \]

\[\hat{x}_{[k]}=\hat{x}_{[k]}^-+K_{[k]}(z_{[k]}-H_m\hat{x}_{[k]}^-) \]

\[e_{[k]}=x_{[k]}-\hat{x}_{[k]}^{-}-K_{[k]}(z_{[k]}-H_{m}\hat{x}_{[k]}^{-}) \]

\[K_{[k]}=\frac{P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}}{H_{\mathrm{m}}P_{[k]}^{-}H_{\mathrm{m}}^{\mathrm{T}}+R_{\mathrm{c}}} \]

\[P_{[k]}=(I-K_{[k]}H_{\mathfrak{m}})P_{[k]}^{-} \]

卡尔曼滤波器算法说明

卡尔曼滤波器算法可以划分为两个部分：时间更新(time update)和测量更新(measurement update) 。

在\(k=1\)时刻，算法开始运行，需要设定初始状态估计\(\hat{x}_{[0]}\)与初始状态估计误差协方差矩阵 \(p_{[0]}\)用作第一组时间更新的参数。

\(\hat{x}_{[0]}\)的选择应尽量靠近可能的真实值，\(p_{[0]}\)则可以选取一个较大的矩阵。

此后，在\(k\) 时刻，时间更新公式首先计算出先验状态估计 \(\hat{x}_{[k]}^-\)与先验状态估计误差协方差矩阵 \(p_{[k]}^-\),这是通过上一次估计和系统模型计算下一次估计值的预测过程。

之后，测量更新公式利用\(P_{[k]}^-\)和\(R_{\mathrm{c}}\)计算出卡尔曼增益\(K_{[k]}\),然后使用卡尔曼增益计算出融合的后验状态估计 \(\hat{x}_{[k]}\),这一过程通过测量值对先验状态估计进行校正。

最后计算出后验状态估计误差协方差矩阵\(P_{[k]}\),为下一时刻(即\(k+1\)时刻)做准备。

卡尔曼滤波器就是重复地进行预测、测量、更新这三个工作。