联邦滤波器在船舶导航中的应用

联邦滤波器在船舶导航中的应用1准备知识联邦滤波器由多个子滤波器和一个主滤波器组成，是一种具有两级数据处理的分散化滤波方法。子滤波器的工作状态与其所受干扰情况密切相关。当干扰大到足以影响滤波器正常工作时，一般就认为子滤波器发生了故障。为了描述子滤波器的工作状态，定义如下新息序列：当子滤波器工作存在干扰时，观测方程为：新息序列也随之变为：2工作原理2.1集中式卡尔曼滤波概述卡尔曼滤波是解决状态最优估计问题的一种常用方法。设随机动态系统的数学模型和有关随机向量的统计性质如下：)()()()()1()1,()1()1,()(kVkXkHkZkWkkkXkkkX式中。RmX是系统状态向量；RmZ是系统观测向量；RmW是系统噪声向量；RmV是观测噪声向量；Rmm*是系统状态转移矩阵；Rmm*是系统噪声矩阵；RmmH*是系统观测矩阵。假定系统噪声)(kW和观测噪声)(kV是不相关的零均值高斯白噪声，初始状态向量)0(X是m维高斯随机向量，且：0)]1()0([,0)]()0([)0()0(var,0)0(0)()([0)1(),()()]1()1([0)(),()()]()([kvXEkwXEPXEXkvkwEkRjkkRjvkvEKQjkkQjwkwETTXTTT随机系统的状态估计问题，就是根据选定的估计准则和获得的量测信息，对系统状态进行估计。其中状态估计确定了被估计的随机状态的转移过程，估计准则确定了状态估计最优性的含义，卡尔曼滤波的估计准则是：估计量)(ˆkX是)(KX的无偏估计，即)()(ˆkEXkXE；同时)(ˆkX也是)(KX的最小方差估计，即：min)](~)(~[)](~)([)](~[kXkXEkXkXJkXJT。根据这两个准则推导出系统的完整的滤波算法。下面给出最终的结果：滤波方程：)]1(ˆ)1()[1()1(ˆ)1(ˆkkZkZkKkkXkX预测方程：)1(ˆ)1()1(ˆ)(ˆ),1()1(ˆkkXkHkkZkXkkkkX增益矩阵：1)]1()1()1()1()[1()1()1(kRkHkkPkHkHkkPkKTT预测误差协方差阵：),1()(),1(),1()(),1()1(kkkQkkkkkPkkkkPTT波误差协方差阵：)1())1()1(()1(kkPkHkKIkPmx初值为)0()0(ˆEXX,)0(var)0(XP。卡尔曼滤波算法是一组递推算法。计算最优滤波值，只需要即时的观测值，无需存储以前的观测数据。利用上一时刻的最优估计值)(ˆkX，由预测方程求得下一时刻的一步预测值)1(~kkX，)1(ˆkkZ；当前测量值)1(kZ和)1(ˆkkZ之差，即新息，与)1(kK相乘对)1(~kkX进行修正，得出k+1时刻的最优估计值)1(ˆkX。如此反复递推运算，但这个计算顺序不是唯一的。在该计算中，我们把预测下一时刻的)1(~kkX和)1(kkP称为滤波时间的更新，把得到)1(ˆkX和)1(kP的过程称为滤波的测量更新。2.2联邦滤波基本理论卡尔曼滤波及联邦滤波一个重要的应用是高精度船舶导航系统。现在可供运载体使用的导航装备很多，选择余地很大。由于采用各种原理实现导航设备的增加使量测信息增多，这对提高船舶导航系统精度提出了很高的要求2.2.1联邦滤波器结构联邦滤波器是一种信息融合技术，其结构可通过图1来说明，它由若干个子滤波器和一个主滤波器组成，是一种具有两级数据处理的特殊的分散化滤波方法，其特殊性在于联邦滤波器采用的是信息分配原理。图中公共参考系统的输出一方面直接给主滤波器，另一方面给各子滤波器作为公共状态变量。各子滤波器的局部估计值iXˆ（公共状态）及其协方差阵iP送入主滤波器和主滤波器的估计值一起进行融合以得到全局最优估计gXˆ和相应的协方差阵gP。图1联邦滤波器的一般结2.2.2联邦滤波器算法设gX、gP表示联邦滤波器的最优估计值和方差，Xi、Pi表示第i个子滤波器的估计值和方差（i=1，2...n），mX、mP表示主滤波器的估计值和方差。全局状态方程为：)()1,()()1,()(kwkkkxkkkx其中：)1,(kk是系统的状态转移矩阵，)1,(kk是系统的噪声矩阵。设)(kW是零均值的白噪声序列，其协方差阵为)(kQ。第i个子系统的量测方程为：)()()()(kvkxkkiiidHZ参考系统子系统1子系统2子系统N局部滤波器1局部滤波器2局部滤波器N主滤波器时间更新最优组合1Z2ZNZggPX11,ˆggPX12,ˆgNgPX1,ˆgPX,ˆ122,ˆPXNNPX,ˆmmPX,ˆgmgPX1,ˆggPX,ˆ其中：Zi(k)是第i个量测传感器的量测值，Hi（k）是第i个量测传感器的量测阵，Vi（k）是传感器的观测噪声阵，是独立于W(k)的零均值白噪声序列，其协方差阵为R（k）。信息分配过程：信息分配就是在各子滤波器和主滤波器之间分配系统的信息。系统的过程信息)(1kQ和)(1kP按如下的信息分配原则在各子滤波器和主滤波器之间进行分配：)()(11kQkQi，)()(1kPkPgii，)()(kXkXgi其中0i是信息分配因子，并满足信息分配原理：mimi11在系统噪声分配过程中，采用了方程上界技术，得到的子滤波器的滤波结果是次优的。物理意义上是由于过程噪声方差被上界取代，则每个滤波器更少的依靠时间更新后的状态值，更多的依靠测量更新。信息的时间更新：时间更新过程在各子滤波器和主滤波器之间独立进行，各子滤波器和主滤波器的滤波算法为：),1()(),1(),1()(),1()1()(ˆ),1()1(ˆkkkQkkkkkPkkkkPkXkkkkXTiTiiii（13）信息的测量，由于主滤波器没有测量值，所以主滤波器没有测量更新。测量更新只在各个局部子滤波器中进行，测量更新通过下式起作用：)()()()1(ˆ)1()(ˆ)()()()()1()(111111kZkRkHkkXkkPkkXkkPkHkRkHkkPkkPiiTiiiiiiiTiii信息融合：联邦滤波器的核心算法是将各个局部滤波器的局部估计信息按下式进行融合，以得到全局的最优估计。mmnnggmngXPXPXPXPPXPPPPPˆˆˆˆ[ˆ][112121111111211通过以上的信息分配、添加（时间更新和测量更新）和信息融合过程，在局部滤波器中由于方差上界技术引起的信息丢失，在融合过程中这种非最优性被重新合成，得到全局最优解。3船舶航行姿态系统中联邦滤波器设计3.1船舶航行系统中联邦滤波器结构在多传感器组合导航系统中，联邦滤波器能够利用信息分配原理实现多传感器信息的最优估计，同时使整个系统具有一定的容错能力。考虑到各个子系统的导航精度都较高，在船舶航行系统中，各子系统容易出现故障，为确保组合系统具有高容错性能，本文采用的是如图3-1所示的联邦滤波器无复位结构。子滤波器选择GPS（全球定位系统）、SINS（惯性导航系统）、TAN（地形辅助性导航系统）和北斗双星子系统。GPS子系统量测信息与SINS系统组成子滤波器1；北斗双星子系统量测信息与SINS系统组成子滤波器2，TAN子系统量测信息与SINS系统组成子滤波器3；主滤波器的输出是所有子系统与SINS系统的量测所确定的全局最优估计。图2联邦滤波器结构图中11ˆ,ˆPX,22ˆ,ˆPX,33ˆ,ˆPX分别为三个子滤波器的状态变量和状态协方差阵；ggPXˆ,~为主滤波器状态变量和主滤波器状态协方差阵。系统选取的全局状态变量为TNEKSVVx][式中：为纬度弧长；为经度弧长；EV为海流东向速度分量；NV为海流北向速度分量；S为船舶相对水的速度；K为船舶航向。动态方程为：65432sinwKwSwVVwVVwKSVNNEEE其中：1w～6w为独立的高斯白噪声；海流用一阶马尔可夫过程来表示；为相关时间常数。全局状态方程)()/1()()/1()1(kwkkkxkkkxGPSBDTAN子滤波器1子滤波器2子滤波器3主滤波器时间更新信息融合日11,ˆPX22,ˆPX33,ˆPXggPX,ˆ状态转移矩阵1000000100000000000000cossin0110sincos1001)/1(TTTTeeTKSTKeTKSTKekk图3联邦滤波器流程图设动态系统为:)(100010001)1(9.00004.00001)(kwkxkx观测方程为：)()(2.00002.00002.0)(kvkxkz3.2无重置估计仿真结果：图4无重置仿真状态X1仿真曲线图5无重置仿真状态X2仿真曲线图6无重置仿真状态X3仿真曲线有重置估计仿真上图为400次滤波的状态误差。蓝色为滤波前X曲线，红色为滤波后X曲线，绿色为主滤波器估计值曲线。无重置结构的滤波器受噪声的影响大，系统容错性能差。3.3有重置估计仿真仿真的信息分配系数为3/1111图7有重置仿真状态X1仿真曲线图8有重置仿真状态X1仿真曲线图9有重置仿真状态X3仿真曲线上图为400次滤波的状态误差。蓝色为滤波前X曲线，红色为滤波后X曲线，绿色为主滤波器估计值曲线。很明显看出有重置的联邦滤波器能够更好地滤除噪声干扰，子滤波器的得到的估计值相似性非常高，可以更好地降低某一个子滤波器因为故障或者抗干扰能力差对系统造成的影响，极大提高了系统的容错性，极大地提高了系统的稳定性。附件无重置滤波仿真程序：clearN=400;w(1)=0;w=randn(1,N)x1(1)=0;x2(1)=0;x3(1)=0;a=1;fork=2:N;x1(k)=x1(k-1)+w(k-1);x2(k)=0.4*x2(k-1)+w(k-1);x3(k)=0.9*x3(k-1)+w(k-1);endV=randn(1,N);q1=std(V);Rvv=q1.^2;q3=std(w);Rww=q3.^2;a=1;b=0.4;c=0.9;d=0.2;Y1=d*x1+V;Y2=d*x2+V;Y3=d*x3+V;a1(1)=0;a2(1)=0;a3(1)=0;s1(1)=0;s2(1)=0;s3(1)=0;e(1)=1f(1)=1fort=2:N;p1(t)=a.^2*a1(t-1)+Rww;b1(t)=d*p1(t)/(d.^2*p1(t)+Rvv);s1(t)=a*s1(t-1)+b1(t)*(Y1(t)-a*d*s1(t-1));a1(t)=p1(t)-d*b1(t)*p1(t);p2(t)=b.^2*a2(t-1)+Rww;b2(t)=d*p1(t)/(d.^2*p2(t)+Rvv);s2(t)=b*s2(t-1)+b2(t)*(Y2(t)-b*d*s2(t-1));a2(t)=p2(t)-d*b2(t)*p1(t);p3(t)=c.^2*a3(t-1)+Rww;b3(t)=d*p3(t)/(d.^2*p1(t)+Rvv);s3(t)=c*s3(t-1)+b3(t)*(Y3(t)-c*d*s3(t-1));a3(t)=p3(t)-d*b3(t)*p1(t);e(t)=1/(1/a1(t)+1/a2(t)+1/a3(t));f(t)=e(t)*(s1(t)/a1(t)+s2(t)/a2(t)+s3(t)/a3(t))endt=1:N