联邦卡尔曼滤波

联邦Kalman滤波

利用Kalman滤波技术对多传感器数据进行最优融合有两种途径:集中式Kalman滤波和分散化滤波。集中式Kalman滤波利用一个滤波器来集中处理所有子系统的信息。分散化滤波利用子滤波器处理子系统的信息通过全局滤波器实现所有子系统的信息融合。

联邦滤波器一般结构

各子滤波器的估计不相关时的融合算法

定理1：若有$N$个局部状态估计$\widehat{X_1}$,$\widehat{X_2}$,$...$,$\widehat{X_N}$和相应的估计误差协方差阵$P_{11}$,$P_{22}$,$...$,$P_{NN}$,且各局估计互不相关，即$P_{ij}=0(i\ne j)$,则全局最优估计可表示为：
$$\widehat{X_g}=P_g\sum _{i=1}^N{P}_{ii}^{-1}\widehat{X_i}$$
其中
$$P_g=(\sum _{i=1}^N{P}_{ii}^{-1}{)}^{-1}$$
由$P_{ii}^{-1}\widehat{X_i}$知，当$\widehat{X_i}$的估计精度差，即$P_{ii}$大时，$\widehat{X_i}$在全局估计的作用比较小。

各子滤波器估计相关条件下的联邦滤波算法

在一般情况下，各子滤波器是估计是相关的。为了解决这个问题，采用方差上界技术,对滤波过程进行适当的改造，使得局部估计实际上不相关,从而进行滤波。

假设各子滤波器的状态估计表示为：
$$\widehat{X_i}=\left[\begin{array}{c}\widehat{X_{ai}}\\ \widehat{X_{bi}}\end{array}\right]$$
其中，$\widehat{X_{ai}}$是各子滤波器的公共状态$X_c$的估计，如导航中的位置，速度，和姿态等误差状态的估计；$\widehat{X_{bi}}$是第$i$个滤波器专有的状态估计，如GPS误差状态的估计。这里只对公共状态估计进行融合以得到其全局估计。

信息分配原则

系统中有两类信息

• 状态运动方程信息

  状态方程的信息量是与系统噪声的方差成反比的，因此可用$Q^{-1}$表示状态状态方程信息量；

• 量测方程信息

  量测方程的信息可用量测噪声协方差的逆$R^{-1}$表示。

将系统噪声总的信息$Q^{-1}$分配到各个子滤波器和主滤波中：
$$Q^{-1}=\sum _{i=1}^N{Q}_i^{-1}+Q_m^{-1}$$
其中$Q_i={\beta }_i^{-1}Q$,$Q_m={\beta }_m^{-1}Q$。

由信息守恒原理可知：
$$\sum _{i=1}^N{\beta }_i+{\beta }_m=1$$
由子滤波器与主滤波器合成的全局估计值$\widehat{X_g}$及其相应的估计误差方差阵$P_g$被${\beta }_i^{-1}{P}_g$后再反馈到子滤波器，重置子滤波器的估计值。
$$\widehat{X_i}=\widehat{X_g}$$

$$P_{ii}={\beta }_i^{-1}{P}_g$$

其中${\beta }_i^{-1}$表示信息分配系数。

为了使$P_{ij}(i\ne j)$恒等于零，则需要对滤波过程进行改造。先构造一个增广系统，它的状态向量由$N$个局部滤波子系统和主滤波子系统的状态组合而成：
$$X=\left[\begin{array}{c}{X}_1\\ ⋮\\ X_{\bar{N}}\end{array}\right]$$
式中：$\bar{N}=N+1$

每个子系统的状态向量为：
$$X_i=\left[\begin{array}{c}{X}_c\\ X_{bi}\end{array}\right]$$
其中，$X_c$是公共状态向量；$X_{bi}$是第$i$个子系统的专有状态。

在这个增广系统的状态向量中含有公共状态，但是不影响理论分析。增广系统的状态方程为
$${\left[\begin{array}{c}{X}_1\\ ⋮\\ X_{\bar{N}}\end{array}\right]}_{k+1}={\left[\begin{array}{ccc}{\varphi }_{11}& & \\ & \ddots & \\ & & {\varphi }_{\bar{N}\bar{N}}\end{array}\right]}_k{\left[\begin{array}{c}{X}_1\\ ⋮\\ X_{\bar{N}}\end{array}\right]}_k+\left[\begin{array}{c}{G}_1\\ ⋮\\ G_{\bar{N}}\end{array}\right]W_k$$

$$E\left[\begin{array}{c}{W}_i{W}_i^T\end{array}\right]=Q$$

增广系统总体滤波误差方差阵一般可表示为
$$P=\left[\begin{array}{ccc}{P}_{11}& \cdots & P_{1\bar{N}}\\ ⋮& & ⋮\\ P_{\bar{N}1}& \cdots & P_{\bar{N}\bar{N}}\end{array}\right]$$
式中：$P_{ji}(j,i=1,2,\cdots ,\bar{N})$表示局部滤波之间的相关性。

考虑到集中滤波的时间更新，由状态方程可知
$$P_{ji}={\varphi }_{jj}{P}_{ji}^{\ast }{\varphi }_{ii}^T+G_{j}Q{G}_i^T$$
由上式知，由于公共噪声$Q$的存在，即使$P_{ji}^{\ast }=0$，$P_{ji}$也不会等于0，即时间更新过程将引入各子滤波器的估计相关。因此需要利用方差上届技术来进行消除。将过程噪声改写为
$$\left[\begin{array}{c}{G}_1\\ ⋮\\ G_{\bar{N}}\end{array}\right]Q\left[\begin{array}{ccc}{G}_1^T& \cdots & G_{\bar{N}}^T\end{array}\right]=\left[\begin{array}{ccc}{G}_1& & \\ & \ddots & \\ & & G_{\bar{N}}\end{array}\right]\left[\begin{array}{ccc}Q& \cdots & Q\\ ⋮& & ⋮\\ Q& \cdots & Q\end{array}\right]\left[\begin{array}{ccc}{G}_1^T& & \\ & \ddots & \\ & & G_{\bar{N}}^T\end{array}\right]$$
由矩阵理论可知：
$$\left[\begin{array}{ccc}Q& \cdots & Q\\ ⋮& & ⋮\\ Q& \cdots & Q\end{array}\right]\le \left[\begin{array}{ccc}{\gamma }_{1}Q& \cdots & 0\\ ⋮& & ⋮\\ 0& \cdots & {\gamma }_{\bar{N}}Q\end{array}\right]$$

$$\frac{1}{{\gamma }_1}+\cdots +\frac{1}{{\gamma }_{\bar{N}}}=1,0\le \frac{1}{{\gamma }_i}\le 1$$

由此可见，选择等号放大估计误差方差矩阵，
$$P_{ii}={\varphi }_{ii}{P}_{ii}^{\ast }{\varphi }_{ii}^T+{\gamma }_i{G}_{i}Q{G}_i^T$$

$$P_{ji}={\varphi }_{jj}{P}_{ji}^{\ast }{\varphi }_{ii}^T=0,P_{ji}^{\ast }=0$$

通过式中表明，只要将各子滤波器的自身的初始方差矩阵在放大些就可以忽略各子滤波器初始方差之间的相关项。这样得到的局部滤波结果也是保守的。

联邦滤波器的设计步骤

1. 将子滤波器和主滤波器的初始估计误差方差矩阵设置为组合系统初始估计误差方差的${\gamma }_i(i=1,2,\cdots ,N)$倍。
2. 将子滤波器和主滤波器的过程噪声方差矩阵设置为组合系统过程噪声方差矩阵的${\gamma }_i$倍。
3. 各子滤波器处理自己的观测信息,进而获得局部估计。
4. 在得到各子滤波器的局部估计和主滤波器的估计后,按定理1进行最优合成。
5. 用全局最优滤波解来重置各子滤波器和主滤波器的滤波值和估计误差方差矩阵。

联邦滤波器的结构

根据信息分配策略不同,联邦滤波算法有4种实现模式:零复位模式；变比例模式；无反馈模式；融合—反馈模式。

1. 零复位模式（${\beta }_m=1,{\beta }_i=0$零化式重置）

优缺点：

• 主滤波器分配到全部信息;
• 故障检测和隔离能力强;
• 减少了数据通讯量，计算简单，解算精度不高。

2. 变比例模式（${\beta }_m={\beta }_i=1/(N+1)$有重置）

优缺点：

• 主滤波器与子滤波器之间平均分配信息;
• 融合后全局滤波精度高，局部滤波因为有全局滤波反馈，精度也提高了;
• 一个子系统发生故障后，主滤波器受污染,隔离后必须重新初始化主滤波器。

3. 无反馈模式（${\beta }_m=0,{\beta }_i=1/N$无重置）

• 主滤波器状态方程无信息分配，主滤波器不需要进行滤波，只起简单的融合作用；
• 各局部滤波器独立滤波，没有反馈重置带来的相互影响提高了容错性能;
• 由于没有全局估计的反馈重置，局部估计精度不高。

4. 融合—反馈模式（${\beta }_m=0,{\beta }_i=1/N$有重置）

优缺点：

• 具有反馈作用，精度较高;
• 滤波器的故障检测与隔离能力差。