集中和分散KF(详细推导)

集中式KF

综合考虑主导航系统及各子导航系统，取多传感器组合导航系统的状态方程为：
$${\mathbf{X}}_{k+1}={\mathbf{\Phi }}_{k+1,k}{\mathbf{X}}_k+{\mathbf{\Gamma }}_k{\mathbf{W}}_k\text{\hspace{1em}(1)}$$
各子导航系统的量测方程可表示为：
$${\mathbf{Z}}_{k+1}^i={\mathbf{H}}_{k+1}^i{\mathbf{X}}_{k+1}+{\mathbf{V}}_{k+1}^i\text{\hspace{1em}(2)}$$

其中,${\mathbf{Z}}_{k+1}^i\in {\mathbf{R}}^{m_i}$为量测向量,${\mathbf{H}}_{k+1}^i$为量测矩阵,${\mathbf{V}}_{k+1}^i\in {\mathbf{R}}^{m_i}$为均值为零、相互独立的高斯序列, 且:

$$\begin{array}{r}\mathrm{E}\left\{\left[\begin{array}{l}{\mathbf{W}}_k\\ {\mathbf{V}}_k^i\end{array}\right]\left[\begin{array}{ll}{\mathbf{W}}_l^{\mathrm{T}}& {\left({\mathbf{V}}_l^i\right)}^{\mathrm{T}}\end{array}\right]\right\}=\left[\begin{array}{cc}{\mathbf{Q}}_{kl}& 0\\ 0& {\mathbf{R}}_{kl}^i\end{array}\right]{\delta }_{kl}\end{array}$$

于是集中式卡尔曼滤波器量测方程为:

$${\mathbf{Z}}_{k+1}={\mathbf{H}}_{k+1}{\mathbf{X}}_{k+1}+{\mathbf{V}}_{k+1}\text{\hspace{1em}(3)}$$

其中，

$$\begin{array}{rl}{\mathbf{Z}}_{k+1}& ={\left[{\left({\mathbf{Z}}_{k+1}^1\right)}^{\mathrm{T}},{\left({\mathbf{Z}}_{k+1}^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathbf{Z}}_{k+1}^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}\\ {\mathbf{H}}_{k+1}& ={\left[{\left({\mathbf{H}}_{k+1}^1\right)}^{\mathrm{T}},{\left({\mathbf{H}}_{k+1}^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathbf{H}}_{k+1}^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}\\ {\mathbf{V}}_{k+1}& ={\left[{\left({\mathbf{V}}_{k+1}^1\right)}^{\mathrm{T}},{\left({\mathbf{V}}_{k+1}^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathbf{V}}_{k+1}^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}\end{array}$$

且

$$\begin{array}{r}\mathrm{E}\left\{\left[\begin{array}{c}{\mathbf{W}}_k\\ {\mathbf{V}}_k\\ {\tilde{\mathbf{X}}}_{0\mid 0}\end{array}\right]\left[\begin{array}{lll}{\mathbf{W}}_l^{\mathrm{T}}& {\mathbf{V}}_l^{\mathrm{T}}& {\tilde{\mathbf{X}}}_{0\mid 0}^{\mathrm{T}}\end{array}\right]\right\}=\left[\begin{array}{ccc}{\mathbf{Q}}_{kl}{\delta }_{kl}& 0& 0\\ 0& {\mathbf{R}}_{kl}{\delta }_{kl}& \\ 0& 0& {\mathbf{P}}_{0\mid 0}\end{array}\right]\end{array}$$

其中,
$${\mathbf{R}}_{k+1}^{-1}=\mathrm{diag}\left\{{\left({\mathbf{R}}_{k+1}^1\right)}^{-1},{\left({\mathbf{R}}_{k+1}^2\right)}^{-1},\cdots ,{\left({\mathbf{R}}_{k+1}^N\right)}^{-1}\right\}\text{\hspace{1em}(4)}$$
集中卡尔曼滤波器的状态估计方程为:
$${\hat{\mathbf{X}}}_{k+1\mid k+1}={\hat{\mathbf{X}}}_{k+1\mid k}+{\mathbf{K}}_{k+1}\left[{\mathbf{Z}}_{k+1}-{\mathbf{H}}_{k+1}{\hat{\mathbf{X}}}_{k+1\mid k}\right]\text{\hspace{1em}(5)}$$

$${\hat{\mathbf{X}}}_{k+1\mid k}={\mathbf{\Phi }}_{k+1,k}{\hat{\mathbf{X}}}_{k\mid k}\text{\hspace{1em}(6)}$$

$$\begin{array}{rl}{\mathbf{K}}_{k+1}& ={\mathbf{P}}_{k+1\mid k+1}{\mathbf{H}}_{k+1}^{\mathrm{T}}{\mathbf{R}}_{k+1}^{-1}\\ & ={\mathbf{P}}_{k+1\mid k+1}\left[{\left({\mathbf{H}}_{k+1}^1\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^1\right)}^{-1},\cdots ,{\left({\mathbf{H}}_{k+1}^N\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^N\right)}^{-1}\right]\\ & =\left[{\mathbf{K}}_{k+1}^1,\cdots ,{\mathbf{K}}_{k+1}^N\right]\end{array}\text{\hspace{1em}(7)}$$

$$\begin{array}{rl}{\mathbf{P}}_{k+1\mid k+1}^{-1}& ={\mathbf{P}}_{k+1\mid k}^{-1}+{\mathbf{H}}_{k+1}^{\mathrm{T}}{\mathbf{R}}_{k+1}^{-1}{\mathbf{H}}_{k+1}\\ & ={\mathbf{P}}_{k+1\mid k}^{-1}+\sum _{i=1}^N{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i\\ & ={\mathbf{P}}_{k+1\mid k}^{-1}+\sum _{i=1}^N\left[{\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}-{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}\right]\end{array}\text{\hspace{1em}(8)}$$

$${\mathbf{P}}_{k+1\mid k}={\mathbf{\Phi }}_{k+1,k}{\mathbf{P}}_{k\mid k}{\mathbf{\Phi }}_{k+1,k}^{\mathrm{T}}+{\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^{\mathrm{T}}\text{\hspace{1em}(9)}$$

其中, 式(8)中 ${\mathbf{P}}_{k+1\mid k+1}$ 也可表示为:

$${\mathbf{P}}_{k+1\mid k+1}=\left[\mathbf{I}-{\mathbf{K}}_{k+1}{\mathbf{H}}_{k+1}\right]{\mathbf{P}}_{k+1\mid k}\text{\hspace{1em}(10)}$$

综合以上, 得到多传感器系统的集中状态估计表达式为:

$${\hat{\mathbf{X}}}_{k+1\mid k+1}={\hat{\mathbf{X}}}_{k+1\mid k}+\sum _{i=1}^N{\mathbf{K}}_{k+1}^i\left[{\mathbf{Z}}_{k+1}^i-{\mathbf{H}}_{k+1}^i{\hat{\mathbf{X}}}_{k+1\mid k}^i\right]\text{\hspace{1em}(11)}$$

分散式KF的一种表达形式

根据式(1)和式(2), 与第$i$个子导航传感器对应的卡尔曼滤波方程为:
$${\hat{\mathbf{X}}}_{k+1\mid k+1}^i={\hat{\mathbf{X}}}_{k+1\mid k}^i+{\mathbf{K}}_{k+1}^i\left[{\mathbf{Z}}_{k+1}^i-{\mathbf{H}}_{k+1}^i{\hat{\mathbf{X}}}_{k+1\mid k}^i\right]\text{\hspace{1em}(12)}$$

$${\mathbf{K}}_{k+1}^i={\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}\text{\hspace{1em}(13)}$$

$${\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}={\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}+{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i\text{\hspace{1em}(14)}$$

$${\hat{\mathbf{X}}}_{k+1\mid k}^i={\mathbf{\Phi }}_{k+1,k}{\hat{\mathbf{X}}}_{k\mid k}^i\text{\hspace{1em}(15)}$$

$${\mathbf{P}}_{k+1\mid k}^i={\mathbf{\Phi }}_{k+1,k}{\mathbf{P}}_{k\mid k}^i{\mathbf{\Phi }}_{k+1,k}^{\mathrm{T}}+{\mathbf{\Gamma }}_k{\mathbf{Q}}_k{\mathbf{\Gamma }}_k^T\text{\hspace{1em}(16)}$$

展开式(5)右边, 合并后有:

$${\hat{\mathbf{X}}}_{k+1\mid k+1}=\left[\mathbf{I}-{\mathbf{K}}_{k+1}{\mathbf{H}}_{k+1}\right]{\hat{\mathbf{X}}}_{k+1\mid k}+{\mathbf{K}}_{k+1}{\mathbf{Z}}_{k+1}\text{\hspace{1em}(17)}$$
由式10可得：
$$\mathbf{I}-{\mathbf{K}}_{k+1}{\mathbf{H}}_{k+1}={\mathbf{P}}_{k+1\mid k+1}{\mathbf{P}}_{k+1\mid k}^{-1}\text{\hspace{1em}(18)}$$
从式(12)可推出:

$$\begin{array}{rl}{\hat{\mathbf{X}}}_{k+1\mid k+1}^i=& \left[\mathbf{I}-{\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i\right]{\hat{\mathbf{X}}}_{k+1\mid k}^i\\ & +{\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i\end{array}\text{\hspace{1em}(19)}$$

这里隐含假定所有出现的矩阵求逆都是存在的, 并且 ${\mathbf{P}}_0$为非奇异的。由式(7)可得:

$$\begin{array}{rl}{\mathbf{K}}_{k+1}{\mathbf{Z}}_{k+1}& ={\mathbf{P}}_{k+1\mid k+1}{\mathbf{H}}_{k+1}^{\mathrm{T}}{\mathbf{R}}_{k+1}^{-1}{\mathbf{Z}}_{k+1}\\ & ={\mathbf{P}}_{k+1\mid k+1}\left[{\left({\mathbf{H}}_{k+1}^1\right)}^{\mathrm{T}},{\left({\mathbf{H}}_{k+1}^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathbf{H}}_{k+1}^N\right)}^{\mathrm{T}}\right]\cdot \\ & \mathrm{diag}\left\{{\left({\mathbf{R}}_{k+1}^1\right)}^{-1},{\left({\mathbf{R}}_{k+1}^2\right)}^{-1},\cdots ,{\left({\mathbf{R}}_{k+1}^N\right)}^{-1}\right\}\cdot \\ & {\left[{\left({\mathbf{Z}}_{k+1}^1\right)}^{\mathrm{T}},{\left({\mathbf{Z}}_{k+1}^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathbf{Z}}_{k+1}^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}\\ & ={\mathbf{P}}_{k+1\mid k+1}\sum _{i=1}^N{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i\end{array}\text{\hspace{1em}(20)}$$

对于式(20)中 ${\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i$ 部分, 利用式(19)可得:

$$\begin{array}{rl}{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i=& {\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k+1}^i-{\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}\cdot \\ & \left[\mathbf{I}-{\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i\right]{\hat{\mathbf{X}}}_{k+1\mid k}^i\end{array}\text{\hspace{1em}(21)}$$

类似于式(18), 有:

$$\mathbf{I}-{\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i={\mathbf{P}}_{k+1\mid k+1}^i{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}\text{\hspace{1em}(22)}$$

将式(22)代入式(21)有:

$${\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i={\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k+1}^i-{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}^i\text{\hspace{1em}(23)}$$

将式(18)、式(20)、式(23)代入式(17)有:

$${\hat{\mathbf{X}}}_{k+1\mid k+1}={\mathbf{P}}_{k+1\mid k+1}\left\{{\mathbf{P}}_{k+1\mid k}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}+\sum _{i=1}^N\left[{\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k+1}^i-{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}^i\right]\right\}\text{\hspace{1em}(24)}$$

为此, 当式(12)-(16)给出了子滤波器的状态估计时, 式(24)代表了$N$个子导航系统在融合中心的最优估计的表达形式。式(24)中 ${\mathbf{P}}_{k+1\mid k+1}、{\mathbf{P}}_{k+1\mid k}、{\hat{\mathbf{X}}}_{k+1\mid k}$ 分别由式(8)、式(9)和式(6)给出。

对于与第$i$个子导航传感器对应的子滤波器$i$ , 如果在$k+1$时刻检测到故障发生, 则该子滤波器只进行时间更新, 即 ${\hat{\mathbf{X}}}_{k+1\mid k+1}^i={\hat{\mathbf{X}}}_{k+1\mid k}^i$ , ${\mathbf{P}}_{k+1\mid k+1}^i={\mathbf{P}}_{k+1\mid k}^i$ 。此时, 根据式(24), 第$i$个子滤波器不参与系统级的信息融合, 进而对第$i$个子滤波器进行隔离, 并自动对系统进行重组 。
以上分析说明, 式(24)的分散模型不仅具有集中KF的最优特性, 同时具有联邦KF较好的容错特性。

总结(分散式KF两种形式)

$$\begin{array}{rl}{\mathbf{P}}_{k+1\mid k+1}^{-1}& ={\mathbf{P}}_{k+1\mid k}^{-1}+\sum _{i=1}^N{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{H}}_{k+1}^i\\ {\hat{\mathbf{X}}}_{k+1\mid k+1}& ={\mathbf{P}}_{k+1\mid k+1}\left\{{\mathbf{P}}_{k+1\mid k}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}+\sum _{i=1}^N\left[{\left({\mathbf{H}}_{k+1}^i\right)}^{\mathrm{T}}{\left({\mathbf{R}}_{k+1}^i\right)}^{-1}{\mathbf{Z}}_{k+1}^i\right]\right\}\end{array}\text{\hspace{1em}(25)}$$

$$\begin{array}{rl}{\mathbf{P}}_{k+1\mid k+1}^{-1}& ={\mathbf{P}}_{k+1\mid k}^{-1}+\sum _{i=1}^N\left[{\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}-{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}\right]\\ {\hat{\mathbf{X}}}_{k+1\mid k+1}& ={\mathbf{P}}_{k+1\mid k+1}\left\{{\mathbf{P}}_{k+1\mid k}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}+\sum _{i=1}^N\left[{\left({\mathbf{P}}_{k+1\mid k+1}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k+1}^i-{\left({\mathbf{P}}_{k+1\mid k}^i\right)}^{-1}{\hat{\mathbf{X}}}_{k+1\mid k}^i\right]\right\}\end{array}\text{\hspace{1em}(26)}$$