CN101216319A - 基于联邦ukf算法的低轨卫星多传感器容错自主导航方法 - Google Patents

Abstract

一种基于联邦UKF算法的低轨卫星多传感器容错自主导航方法，属卫星自主导航方法。该方法包括：建立直角坐标系的地球卫星轨道动力学方程；建立以星敏感器和红外地球敏感器的输出值为测量量的子系统量测方程；建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程；建立以紫外敏感器的输出值为量测量的子系统量测方程；选择Sigma采样点；建立离散型UKF算法的预测方程和更新方程；各子系统分别独立进行Sigma采样点计算及预测更新和量测更新；根据预测滤波残差判断各子滤波器输出是否有效，如有故障将其隔离，否则将滤波结果输入主滤波器进行信息融合；建立基于UKF算法的无复位联邦UKF滤波方程；按照上述步骤，输出为地球卫星状态估计值及方差矩阵P。

Description

基于联邦UKF算法的低轨卫星多传感器容错自主导航方法

技术领域

本发明属于卫星多传感器信息融合自主导航技术领域，涉及一种基于联邦UKF算法的卫星多传感器容错自主导航方法，可用于近地轨道地球卫星的自主导航定位。

背景技术

卫星自主导航是指卫星在不依赖地面站的条件下，仅依靠卫星上的测量设备实时确定卫星的位置和速度，在轨完成的飞行任务要求的功能或操作。航天器通常采用的依靠地面站无线电测控进行定位导航的方法，由于受地理条件的限制，难以实现对中低轨道卫星整个轨道的定位导航；同时，随着空间运行的各种航天器的数目的增加，信息传输量剧增，完全依靠地面站测控，将会引起测控系统的信息阻塞和地面站负荷过重。因此，采用自主导航技术，提高卫星的自主运行、自主管理和在轨生存能力，已经成为现代卫星导航系统发展的必然趋势。

在卫星自主导航系统中，为了提高可靠性和精度，通常采用多种传感器同时进行定轨。目前，星上常用的自主导航传感器有以下几类：利用地球物理特性的敏感器，如红外地平仪、磁强计、雷达高度计等；利用天体位置的敏感器，如太阳敏感器、星敏感器、紫外敏感器等；利用惯性的敏感器，如陀螺仪、角加速度计等。这些传感器的测量方程都具有较强的非线性，如何将其信息有效的组织并充分利用，是卫星自主导航的关键问题。传统的信息处理方法是采用集中式扩展卡尔曼滤波器，但是这种方法存在两个缺陷：一是采用集中式处理方式，导致计算负担过重，容错性差、通信负担重；二是这种方法的假设前提十分苛刻，当系统非线性特性较强或对噪声估计不准确时，将会造成滤波器发散，导致导航精度下降。

Unscented卡尔曼滤波(UKF)是一种新的信息处理方法，它采用卡尔曼线性滤波框架，使用UT变换来处理一步预测方程中的均值和协方差的非线性传递，不需要求导计算Jacobian矩阵，算法简单，容易实现，利用UKF算法可以获得比传统的扩展卡尔曼滤波(EKF)更高的估计精度。近年来，基于UKF算法的轨道及姿态确定方法研究得到迅速发展。如何采用信息分配原理，将Unscented卡尔曼滤波算法和信息融合技术相结合，实现基于UKF的多传感器非线性滤波算法，使系统能够充分运用各导航系统的信息进行信息互补和信息融合，并具有故障检测、隔离和重构的功能，对卫星自主导航技术发展有着非常重要的意义。

发明内容

发明目的：本发明的目的是提供一种具有容错功能的近地轨道卫星多传感器自主导航定位方法，该方法能够充分利用卫星上的测量设备提供的导航信息实现自主定轨，并解决了如何用联邦滤波技术对Unscented卡尔曼滤波结果进行信息融合的问题。

技术方案：为达到上述发明目的，本发明设计了一种基于多传感器信息融合的联邦UKF滤波器，该滤波器采用地球卫星的轨道动力学方程作为状态方程，分别以星敏感器+红外地球敏感器、磁强计+雷达高度计、紫外敏感器作为测量器件建立三个子滤波器；用无复位联邦滤波技术对各子滤波器的Unscented卡尔曼滤波结果进行信息融合；用残差检验技术对子滤波器进行故障检测、隔离和重构。本发明可在测量系统非线性较强、对系统噪声和量测噪声估计不准确的情况下获得较高的定轨精度。各个子滤波器独立工作，相互之间没有干扰，既保证了输出导航参数的连续性和稳定性，也便于工程实现，并可通过主滤波器迅速实现系统重构，使因部分导航传感器故障导致子系统失效后，组合导航系统仍能继续工作，具容错性。大大提高了地球卫星导航系统的适用范围。本发明具体步骤如下：

(1)系统初始化；

读取轨道数据初值，状态初始条件为：

${\hat{X}}_0=E\left(X_0\right)---\left(1\right)$

$P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right]---\left(2\right)$

状态矢量X₀＝[x₀，y₀，z₀，v_x0，v_y0，v_z0]^T，x₀，y₀，z₀，v_x0，v_y0，v_z0，分别为卫星在X，Y，Z三个方向的初始位置和速度；并设系统过程噪声w(k)和量测噪声v(k)为加性噪声，Q_k和R_k分别为系统和量测噪声协方差阵。

(2)建立基于直角坐标系的地球卫星轨道运动学方程；

选取历元(J2000.0)地球赤道惯性坐标系，建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过四阶龙格-库塔方法解微分方程计算出地球卫星的位置(x，y，z)和速度(v_x，v_y，v_z)信息。简化的系统状态方程为：

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v_x\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v_y\\ \frac{\mathrm{dz}}{\mathrm{dt}}=v_z\\ \frac{{\mathrm{dv}}_x}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}x}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_x\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}y}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_y\\ \frac{d{y}_z}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}z}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-4.5)]+{\mathrm{\ΔF}}_z\end{array}\right.---\left(3\right)$

式中，R_e为地球参考赤道半径，状态矢量X＝[x，y，z，v_x，v_y，v_z]^T，x，y，z，v_x，v_y，v_z分别为卫星在X，Y，Z三个方向的位置和速度， $r=\sqrt{x^2+y^2+z^2}$ 为地心距，μ是地球引力常数，J₂为地球引力二阶带谐项系数，ΔF_x，ΔF_y，ΔF_z为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些摄动力的影响用系统过程噪声w(k)表示。

对于系统过程噪声为零均值白噪声有：

E{w(k)}＝0，E{w(k)w^T(j)}＝Q_kδ_kj，k，j＝1，2，3…

式中，δ_kj为克朗涅克笛耳塔(Kronecker Delta)函数。

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{cc}1& j=k\\ 0& j\≠k\end{array}\right.$

(3)建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方程；

以星敏感器和红外地球敏感器的子系统的量测量为星光角距，星光角距是指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，星光角距a的量测方程为：

$Z_a=\mathrm{\α}+v_a=\arccos (-\frac{\stackrel{\→}{r}\·\stackrel{\→}{s}}{r})+v_a---\left(4\right)$

式中为卫星在地心惯性坐标系中的位置矢量，由红外地球敏感器获得：

为地球惯性坐标系下的导航星星光方向的单位矢量，由星敏感器识别；r为地心距；v_a为测量误差。

设量测噪声为零均值白噪声：

E{v_a(k)}＝0， $E\left\{v_a\left(k\right)v_a^T\left(j\right)\right\}=R_{k1}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

(4)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程；

磁强计和雷达高度计构成的子系统的量测量为地磁场强度矢量和卫星轨道高度，由于通过磁强计对磁场强度的测量即可单独完成定轨功能，雷达高度计起辅助定位、提高系统精度的作用，因此本子系统对雷达高度计设计了故障检测阈值，当雷达高度计测量值和计算值的差值超出阈值时，将雷达高度计隔离，仅使用磁强计进行定轨，保证了在雷达高度计失效时子系统2仍能正常工作。

三轴磁强计的测量值为卫星所在位置的地磁场矢量

在卫星本体系中的三个分量，为简化量测模型，取地磁场强度矢量的模作为磁测自主导航观测量，比较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息。量测方程为：

$Z_B=\sqrt{B_x^2+B_y^2+B_z^2}=\sqrt{{\left(\frac{1}{r}\frac{\∂V}{\∂\mathrm{\θ}}\right)}^2+{(-\frac{1}{r\sin \mathrm{\θ}}\frac{\∂V}{\∂\mathrm{\φ}})}^2+{\left(\frac{\∂V}{\∂\stackrel{\→}{r}}\right)}^2}+v_B---\left(5\right)$

其中，B_x、B_y、B_z为地磁场矢量

在卫星本体系中的三个分量；V为地磁场势函数，r为地心距，φ为地理经度，θ为地心余纬，

为地理坐标系下的卫星位置矢量，v_B为量测噪声。

设量测噪声为零均值白噪声：E{v_B(k)}＝0， $E\left\{v_B\left(k\right)v_B^T\left(j\right)\right\}=R_{k2}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息，由于海平面很接近于大地水准面，所以可认为雷达高度计测得的卫星高度是卫星至星下点处大地水准面的距离。取测量模型为：

其中，R_e为地球参考赤道半径，为地心纬度，f为地球参考椭球的扁率，v_H(t)为量测噪声。

设量测噪声为零均值白噪声：

E{v_H(k)}＝0， $E\left\{v_H\left(k\right)v_H^T\left(j\right)\right\}=R_{k3}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

从式5和式6可以看出，在地磁场强度和轨道高度的测量模型中，磁场强度和轨道高度是根据卫星在地球固联坐标系下的位置信息计算的，而卫星的位置矢量一般是在地心惯性坐标系中给出的，因此，需建立地心惯性坐标系和地球固联坐标系之间的转换矩阵。从地心赤道惯性坐标系S_i到地球固连坐标系S_e的坐标变换矩阵为：

$L_{\mathrm{ei}}=L_z\left(a_G\right)=\begin{array}{}\end{array}\left[\begin{array}{ccc}{\cos a}_G& {\sin a}_G& 0\\ -{\sin a}_G& {\cos a}_G& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

式中：a_G为Greenwich赤经。

(5)建立以紫外敏感器的输出值为量测量的子系统量测方程；

紫外敏感器的量测值为卫星的位置矢量。紫外敏感器工作在紫外波段，它可以同时观测多个天体目标，提供三轴的航天器姿态信息，定姿精度可达0.05°。另外，通过对地球圆盘的图像处理，可提取星体坐标系中的地心方向和地心距离信息。结合定姿过程得到的姿态信息，可计算出卫星在惯性系中的位置矢量。子系统的量测方程为：

$Z_u=\left[\begin{array}{c}\stackrel{\→}{r}\\ \stackrel{\‾}{r}\\ r\end{array}\right]+v_u---\left(8\right)$

其中，

为卫星在地心惯性坐标系中的位置矢量， $r=\sqrt{x^2+y^2+z^2}$ 为地心距；v_u为测量误差。

设量测噪声为零均值白噪声：

E{v_u(k)}＝0， $E\left\{v_u\left(k\right)v_u^T\left(j\right)\right\}=R_{k4}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

(6)依照最小偏度采样策略选择Sigma采样点；

①选择0≤W₀＜1，Sigma权值为：

$W_i=\left\{\begin{array}{cc}\frac{1-W_0}{2^n}& i=\mathrm{1,2}\\ 2^{i-2}{W}_1& i=3,\·\·\·,n+1\end{array}---\left(9\right)\right.$

$W_i^m=W_i^c=W_i$

式中，W为权值，n为状态方程维数，W_i ^m为均值加权值，W_i ^c为协方差加权值。

②对应状态为1维情况，迭代初始向量：

${\mathrm{\χ}}_0^1=\left[0\right],$ ${\mathrm{\χ}}_1^1=[-\frac{1}{\sqrt{{2W}_1}}],$ ${\mathrm{\χ}}_2^1=\left[\frac{1}{\sqrt{{2W}_1}}\right]---\left(10\right)$

③对于输入维数j＝2，…n时，迭代公式为：

${\mathrm{\χ}}_i^{j+1}=\left\{\begin{array}{cc}\left[\begin{array}{c}{\mathrm{\χ}}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{\mathrm{\χ}}_i^{j-1}\\ -\frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=1,\·\·\·j\\ \left[\begin{array}{c}{0}_{j-1}\\ \frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=j+1\end{array}\right.---\left(11\right)$

(7)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程，各子系统分别独立进行Sigma采样点计算以及预测更新和量测更新；

①预测方程：

对所生成的Sigma点加入状态矢量X的均值和协方差信息：

${\mathrm{\χ}}_i=\stackrel{\‾}{X}+\sqrt{P\left(k\right)}{\mathrm{\χ}}_i^j,$ i＝0，1，2，…，n+1    (12)

χ_i(k+1|k)＝f[χ_i(k|k)]    (13)

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{\mathrm{\χ}}_i(k+1|k)---\left(14\right)$

其中L为采样点个数，

为t_k时刻对t_k+1的预测估值，P(k)为误差协方差阵初值，f(·)为系统状态方程。

$P(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k))}^T+Q_k---\left(15\right)$

Z_i(k+1|k)＝h[χ_i(k+1|k)]    (16)

$\hat{Z}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{Z}_i(k+1|k)---\left(17\right)$

式中，P(k+1|k)为预测估值误差协方差阵，h(·)为量测方程，

为对t_k+1观测值的预测值。

$P_{\mathrm{yy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c(Z_i(k+1|k)-\hat{Z}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T+R_k---\left(18\right)$

$P_{\mathrm{xy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T---\left(19\right)$

②更新方程：

$W(k+1)=P_{\mathrm{xy}}(k+1|k)P_{\mathrm{yy}}^{-1}(k+1|k)---\left(20\right)$

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)(Z(k+1)-\hat{Z}(k+1|k))---\left(21\right)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)P_yy(k+1|k)W^T(k+1)    (22)

式中，W(k+1)为增益矩阵，

为滤波值，P(k+1|k+1)为滤波值误差协方差阵。

(8)利用残差检验法，根据预测滤波残差判断各子滤波器输出值是否有效，如出现故障则将其隔离，否则将滤波结果输入主滤波器进行信息融合；

残差检验法的故障函数为：

其中， ${\mathrm{\χ}}_m^2\left(k\right)\stackrel{\mathrm{\Δ}}{=}{v}_k^T\left(k\right)P_{\mathrm{yy}}^{-1}\left(k\right|k-1)v_k\left(k\right)~{\mathrm{\χ}}_m^2,$ 即χ_m ²(k)为具有m个自由度的χ²变量，m为量测矢量Z的维数；v_k为残差， $v_k=z\left(k\right)-\hat{z}\left(k\right|k-1);$

对于正常滤波，取误警率 $P\{{\mathrm{\χ}}_m^2\left(k\right)>T_D\}={\∫}_{{\mathrm{\χ}}_a^2}^{+\∞}{k}_m\left({\mathrm{\χ}}^2\right)d{\mathrm{\χ}}^2=a;$ 其中k_m(·)为χ_m ²的概率密度函数，a为显著性水平，T_D为由误警率确定的门限值。

(9)建立基于UKF算法的无复位联邦UKF滤波方程；

联邦滤波器的信息融合方式采用无复位模式，仅对状态量进行融合和反馈，各子系统的协方差阵独立进行时间更新和预测更新，不参加主滤波器的融合：

$P_{k+1|k+1}^g={\left[\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}\right]}^{-1}$ l＝1，2，3    (24)

${\hat{x}}_{k+1|k+1}^g=P_{k+1|k+1}^g\left[\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}{\hat{x}}_{k+1|k+1}^i\right]$ l＝1，2，3    (25)

(10)按照上述步骤(1)-(9)，输出为地球卫星状态矢量估计值

及其方差矩阵P^g，其中状态估计值包括地球卫星位置和速度矢量[x，y，z，v_x，v_y，v_z]^T，状态估计方差矩阵P^s包括地球卫星位置和速度估计方差[P_x ^g，P_y ^g，P_z ^g，P_vx ^g，P_vy ^g，P_vz ^g]^T。

3、有益效果：本发明的方法有如下优点：(1)采用星敏感器+红外地球敏感器、磁强计+雷达高度计、紫外敏感器的组合，能够充分利用星上的测量设备提供的信息进行导航定位，具有完全的自主性；(2)子系统2采用基于UKF技术的磁强计和雷达高度计组合定轨，并设计故障检测阈值，保证在雷达高度计失效后系统仍然能继续工作，系统具较高的精度和可靠性；(3)采用UKF算法对子滤波器进行状态估计，在测量系统非线性较强、或对系统噪声和量测噪声估计不准确的情况下均能够获得较高的定轨精度；(4)UKF算法采用最小偏度采样策略，减少了采样点数，在保证精度的前提下提高了计算效率；(5)用无复位联邦滤波技术对各子滤波器的UKF滤波结果进行信息融合，对每个子系统单独计算Sigma采样点，各子滤波器独立工作，相互之间没有干扰，既保证了输出导航参数的连续性和稳定性，也便于工程实现，(6)用残差检验技术对子滤波器进行故障检测，并可在检测到故障时，通过主滤波器将其隔离，迅速实现系统重构，使因部分导航传感器故障导致子系统失效后，组合导航系统仍能继续工作，具容错性。

附图说明：

图1卫星多传感器容错联邦UKF系统流程图；

图2卫星多传感器容错联邦UKF系统结构原理图；

图3标称轨迹示意图；

图4仿真轨迹示意图；

图5标称轨迹位置变化曲线；

图6标称轨迹速度变化曲线；

图7仿真轨迹位置变化曲线；

图8仿真轨迹速度变化曲线；

图9理想情况下联邦UKF滤波误差曲线；

图10对系统噪声估计不足情况下的联邦UKF滤波误差曲线；

图11对量测噪声估计不足情况下的联邦UKF滤波误差曲线；

图12子系统1故障情况下的联邦UKF误差曲线；

图13子系统2故障情况下的联邦UKF误差曲线；

图14子系统3故障情况下的联邦UKF误差曲线；

具体实施方式

本实例的具体组成图如图1所示，实施的流程如图2所示，轨道数据由STK(卫星仿真工具包)软件产生，具体仿真条件如下；

①仿真时间2008年6月1日12:00-2008年6月1日24:00，采样周期T为10s。标称轨道数据为：半长轴：a＝6823.082km；偏心率：e＝1.83×10^-16；轨道倾角：i＝97.2°；升交点赤经：Ω＝157.5°；近升角距：ω＝0°。

②为了模拟卫星在轨运行的真实运动情况，在计算卫星轨道时，使用力学模型JGM-3，并考虑如下摄动因素：地球非球形摄动；太阳引力、月球引力；太阳光压；大气阻力。

③测量仪器精度：导航星采用分布在天球上的80颗最亮的恒星(星等≤2.3)，星敏感器的视场是25°×25°，精度为3″；红外地平仪的精度为0.02°；磁强计测量噪声误差取为50nT，地磁场模型误差取为150nT；雷达高度计测量误差取为200m。紫外敏感器测距误差500m，地心方向测量误差0.01°。

④轨道初始位置误差为5km，初始速度误差取为10m/s。

本实例以3个子系统的量测输出值作为量测量，结合地球卫星的状态方程，分别利用基于最小偏度采样的UKF算法对各子系统单独进行时间更新和量测更新，并利用残差检验法对3个子系统的滤波结果进行分析，判断子系统输出是否有效，如果有效则将该结果输出给主滤波器，利用无复位联邦滤波器在主滤波器中对有效的子滤波器的状态估计值进行信息融合，得到地球卫星的位置、速度信息。

具体的实施步骤如下：

(1)系统初始化；

读取轨道数据初值，状态初始条件为：

${\hat{X}}_0=E\left(X_0\right)---\left(1\right)$

$P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right]---\left(2\right)$

状态矢量X₀＝[x₀，y₀，z₀，v_x0，v_y0，v_z0]^T，x₀，y₀，z₀，v_x0，v_y0，v_z0，分别为卫星在X，Y，Z三个方向的初始位置和速度；并设系统过程噪声w(k)和量测噪声v(k)为加性噪声，Q_k和R_k分别为系统和量测噪声协方差阵。

(2)建立基于直角坐标系的地球卫星轨道运动学方程；

选取历元(J2000.0)地球赤道惯性坐标系，建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过四阶龙格-库塔方法解微分方程计算出地球卫星的位置(x，y，z)和速度(v_x，v_y，v_z)信息。简化的系统状态方程为：

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v_x\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v_y\\ \frac{\mathrm{dz}}{\mathrm{dt}}=v_z\\ \frac{{\mathrm{dv}}_x}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}x}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_x\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}y}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_y\\ \frac{d{y}_z}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}z}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-4.5)]+{\mathrm{\ΔF}}_z\end{array}\right.---\left(3\right)$

式中，R_e为地球参考赤道半径，状态矢量X＝[x，y，z，v_x，v_y，v_z]^T，x，y，z，v_x，v_y，v_z分别为卫星在X，Y，Z三个方向的位置和速度， $r=\sqrt{x^2+y^2+z^2}$ 为地心距，μ是地球引力常数，J₂为地球引力二阶带谐项系数，ΔF_x，ΔF_y，ΔF_z为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些摄动力的影响用系统过程噪声w(k)表示。

对于系统过程噪声为零均值白噪声有：

E{w(k)}＝0，E{w(k)w^T(j)}＝Q_kδ_kj，k，j＝1，2，3…式中，δ_kj为克朗涅克笛耳塔(Kronecker Delta)函数。

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{cc}1& j=k\\ 0& j\≠k\end{array}\right.$

(3)建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方程；

以星敏感器和红外地球敏感器的子系统的量测量为星光角距，星光角距是指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，星光角距a的量测方程为：

$Z_a=\mathrm{\α}+v_a=\arccos (-\frac{\stackrel{\→}{r}\·\stackrel{\→}{s}}{r})+v_a---\left(4\right)$

式中

为卫星在地心惯性坐标系中的位置矢量，由红外地球敏感器获得；为地球惯性坐标系下的导航星星光方向的单位矢量，由星敏感器识别；r为地心距；v_a为测量误差。

设量测噪声为零均值白噪声：

E{v_a(k)}＝0， $E\left\{v_a\left(k\right)v_a^T\left(j\right)\right\}=R_{k1}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

(4)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程；

磁强计和雷达高度计构成的子系统的量测量为地磁场强度矢量和卫星轨道高度，由于通过磁强计对磁场强度的测量即可单独完成定轨功能，雷达高度计起辅助定位、提高系统精度的作用，因此本子系统对雷达高度计设计了故障检测阈值，当雷达高度计测量值和计算值的差值超出阈值时，将雷达高度计隔离，仅使用磁强计进行定轨，保证了在雷达高度计失效时子系统2仍能正常工作。三轴磁强计的测量值为卫星所在位置的地磁场矢量

在卫星本体系中的三个分量，为简化量测模型，取地磁场强度矢量的模作为磁测自主导航观测量，比较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息。量测方程为：

$Z_B=\sqrt{B_x^2+B_y^2+B_z^2}=\sqrt{{\left(\frac{1}{r}\frac{\∂V}{\∂\mathrm{\θ}}\right)}^2+{(-\frac{1}{r\sin \mathrm{\θ}}\frac{\∂V}{\∂\mathrm{\φ}})}^2+{\left(\frac{\∂V}{\∂\stackrel{\→}{r}}\right)}^2}+v_B---\left(5\right)$

其中，B_x、B_y、B_z，为地磁场矢量

在卫星本体系中的三个分量；V为地磁场势函数，r为地心距，φ为地理经度，θ为地心余纬，

为地理坐标系下的卫星位置矢量，v_B为量测噪声。

设量测噪声为零均值白噪声：

E{v_B(k)}＝0， $E\left\{v_B\left(k\right)v_B^T\left(j\right)\right\}=R_{k2}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息，由于海平面很接近于大地水准面，所以可认为雷达高度计测得的卫星高度是卫星至星下点处大地水准面的距离。取测量模型为：

其中，R_e为地球参考赤道半径，为地心纬度，f为地球参考椭球的扁率，v_H(t)为量测噪声。

设量测噪声为零均值白噪声：

E{v_H(k)}＝0， $E\left\{v_H\left(k\right)v_H^T\left(j\right)\right\}=R_{k3}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

从式5和式6可以看出，在地磁场强度和轨道高度的测量模型中，磁场强度和轨道高度是根据卫星在地球固联坐标系下的位置信息计算的，而卫星的位置矢量一般是在地心惯性坐标系中给出的，因此，需建立地心惯性坐标系和地球固联坐标系之间的转换矩阵。从地心赤道惯性坐标系S_i到地球固连坐标系S_e的坐标变换矩阵为：

$L_{\mathrm{ei}}=L_z\left(a_G\right)=\begin{array}{}\end{array}\left[\begin{array}{ccc}{\cos a}_G& {\sin a}_G& 0\\ -{\sin a}_G& {\cos a}_G& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

式中：a_G为Greenwich赤经。

(5)建立以紫外敏感器的输出值为量测量的子系统量测方程；

紫外敏感器的量测值为卫星的位置矢量。紫外敏感器工作在紫外波段，它可以同时观测多个天体目标，提供三轴的航天器姿态信息，定姿精度可达0.05°。另外，通过对地球圆盘的图像处理，可提取星体坐标系中的地心方向和地心距离信息。结合定姿过程得到的姿态信息，可计算出卫星在惯性系中的位置矢量。子系统的量测方程为：

$Z_u=\left[\begin{array}{c}\stackrel{\→}{r}\\ \stackrel{\‾}{r}\\ r\end{array}\right]+v_u---\left(8\right)$

其中，

为卫星在地心惯性坐标系中的位置矢量， $r=\sqrt{x^2+y^2+z^2}$ 为地心距；v_u为测量误差。

设量测噪声为零均值白噪声：

E{v_u(k)}＝0， $E\left\{u_v\left(k\right)v_u^T\left(j\right)\right\}=R_{k4}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…

(6)依照最小偏度采样策略选择Sigma采样点；

①选择0≤W₀＜1，Sigma权值为：

$W_i=\left\{\begin{array}{cc}\frac{1-W_0}{2^n}& i=\mathrm{1,2}\\ 2^{i-2}{W}_1& i=3,\·\·\·,n+1\end{array}---\left(9\right)\right.$

$W_i^m=W_i^c=W_i$

式中，W为权值，n为状态方程维数，W_i ^m为均值加权值，W_i ^c为协方差加权值。

②对应状态为1维情况，迭代初始向量：

${\mathrm{\χ}}_0^1=\left[0\right],$ ${\mathrm{\χ}}_1^1=[-\frac{1}{\sqrt{{2W}_1}}],$ ${\mathrm{\χ}}_2^1=\left[\frac{1}{\sqrt{{2W}_1}}\right]---\left(10\right)$

③对于输入维数j＝2，…n时，迭代公式为：

${\mathrm{\χ}}_i^{j+1}=\left\{\begin{array}{cc}\left[\begin{array}{c}{\mathrm{\χ}}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{\mathrm{\χ}}_i^{j-1}\\ -\frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=1,\·\·\·j\\ \left[\begin{array}{c}{0}_{j-1}\\ \frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=j+1\end{array}\right.---\left(11\right)$

(7)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程，各子系统分别独立进行Sigma采样点计算以及预测更新和量测更新；

①预测方程：

对所生成的Sigma点加入状态矢量X的均值和协方差信息：

${\mathrm{\χ}}_i=\stackrel{\‾}{X}+\sqrt{P\left(k\right)}{\mathrm{\χ}}_i^j,$ i＝0，1，2，…，n+1          (12)

χ_i(k+1|k)＝f[χ_i(k/k)]    (13)

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{\mathrm{\χ}}_i(k+1|k)---\left(14\right)$

其中L为采样点个数，

为t_k时刻对t_k+1的预测估值，P(k)为误差协方差阵初值，f(·)为系统状态方程。

$P(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k))}^T+Q_k---\left(15\right)$

Z_i(k+1|k)＝h[χ_i(k+1)]    (16)

$\hat{Z}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{Z}_i(k+1|k)---\left(17\right)$

式中，P(k+1|k)为预测估值误差协方差阵，h(·)为量测方程，

为对t_k+1观测值的预测值。

$P_{\mathrm{yy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c(Z_i(k+1|k)-\hat{Z}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T+R_k---\left(18\right)$

$P_{\mathrm{xy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T---\left(19\right)$

②更新方程：

$W(k+1)=P_{\mathrm{xy}}(k+1|k)P_{\mathrm{yy}}^{-1}(k+1|k)---\left(20\right)$

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)(Z(k+1)-\hat{Z}(k+1|k))---\left(21\right)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)P_yy(k+1|k)W^T(k+1)    (22)

式中，W(k+1)为增益矩阵，

为滤波值，P(k+1|k+1)为滤波值误差协方差阵。

(8)利用残差检验法，根据预测滤波残差判断各子滤波器输出值是否有效，如出现故障则将其隔离，否则将滤波结果输入主滤波器进行信息融合；

残差检验法的故障函数为：

其中， ${\mathrm{\χ}}_m^2\left(k\right)\stackrel{\mathrm{\Δ}}{=}{v}_k^T\left(k\right)P_{\mathrm{yy}}^{-1}\left(k\right|k-1)v_k\left(k\right)~{\mathrm{\χ}}_m^2,$ 即χ_m ²(k)为具有m个自由度的χ²变量，m为量测矢量Z的维数；v_k为残差， $v_k=z\left(k\right)-\hat{z}\left(k\right|k-1);$

对于正常滤波，取误警率 $P\{{\mathrm{\χ}}_m^2\left(k\right)>T_D\}={\∫}_{{\mathrm{\χ}}_a^2}^{+\∞}{k}_m\left({\mathrm{\χ}}^2\right)d{\mathrm{\χ}}^2=a;$ 其中k_m(·)为χ_m ²的概率密度函数，a为显著性水平，T_D为由误警率确定的门限值。

(9)建立基于UKF算法的无复位联邦UKF滤波方程；

联邦滤波器的信息融合方式采用无复位模式，仅对状态量进行融合和反馈，各子系统的协方差阵独立进行时间更新和预测更新，不参加主滤波器的融合：

$P_{k+1|k+1}^g={\left[\underset{i=0}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}\right]}^{-1}$ l＝1，2，3    (24)

${\hat{x}}_{k+1|k+1}^g=P_{k+1|k+1}^g\left[\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}{\hat{x}}_{k+1|k+1}^i\right]$ l＝1，2，3    (25)

(10)按照上述步骤(1)-(9)，输出为地球卫星状态矢量估计值

及其方差矩阵P^g，其中状态估计值

包括地球卫星位置和速度矢量[x，y，z，v_x，v_y，v_z]^T，状态估计方差矩阵P^g包括地球卫星位置和速度估计方差[P_x ^g，P_y ^g，P_z ^g，P_vx ^g，P_vy ^g，P_vz ^g]^T。

对本发明的有益效果说明如下：

(1)本发明的系统组成图如图1所示，程序流程图如图2所示；

(2)设计轨道为太阳同步轨道，轨道高度500km，降交点地方时上午10:30，轨道倾角97.2°，仿真轨迹如图3-8所示；

(3)为充分说明本发明方法对导航结果的修正作用，分别给出理想状态下的联邦UKF、对系统噪声估计不足情况下的联邦UKF、对量测噪声估计不足情况下的联邦UKF滤波的滤波结果，分别如下所示：

①理想情况下联邦UKF滤波误差曲线如图9所示；

②对系统噪声估计不足情况下的联邦UKF滤波误差曲线如图10所示；

③对量测噪声估计不足情况下的联邦UKF滤波误差曲线如图11所示；

分别对比3种情况下的联邦UKF滤波结果，可以看出：

①在理想情况下，组合导航系统能够迅速收敛，且没有发散现象；在系统收敛后，联邦自适应UKF的位置误差不超过400m，速度误差不超过0.8m/s，能够很好的完成定轨功能。

②当对系统噪声估计不足时，联邦UKF滤波结果所受影响不大，仍能很好的完成定轨功能。

③当对量测噪声估计不足时，联邦UKF滤波结果所受影响不大，仍能很好的完成定轨功能。

④三种情况下的滤波精度如表1所示。

表1不同情况下的联邦UKF在定轨精度比较

滤波结果(1σ)	理想状态	系统噪声估计不足	量测噪声估计不足
				位置精度m速度精度m/s	238.80.65	257.70.73	257.20.73

从以上分析可以看出，本发明的方法本文采用的联邦UKF万法对低轨卫星位置和速度的误差的修正效果良好，该方法适合于系统非线性较强、系统状态噪声阵或量测噪声阵未知的情况。

本发明的采用容错联邦UKF算法来提高非线性系统的稳定性和滤波精度，因此这里对这种手段所起的作用进行说明。

表2子系统故障情况下定轨精度比较

滤波结果(1σ)    无故障    子系统1故障  子系统2故障  子系统3故障

位置精度m        238.8     263.9        264.5        273.3

速度精度m/s      0.65      0.69         0.85         0.81

子系统故障情况下联邦UKF滤波结果如图12-14和表2所示，分别在不同时间对3个子系统加入突变故障，可以看出，当单个子系统故障时，主系统可通过残差检验法迅速检验出故障，并进行系统重构，子系统的故障不会对主系统造成污染，滤波结果变化较小。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于联邦UKF算法的低轨卫星多传感器容错自主导航方法，其特征在于，包括下列步骤：

(一)系统初始化；

读取轨道数据初值，状态初始条件为：

${\hat{X}}_0=E\left(X_0\right)---\left(1\right)$

$P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right]---\left(2\right)$

状态矢量X₀＝[x₀，y₀，z₀，v_x0，v_y0，v_z0]^T，x₀，y₀，z₀，v_x0，v_y0，v_z0，分别为卫星在X，Y，Z三个方向的初始位置和速度；并设系统过程噪声w(k)和量测噪声v(k)为加性噪声，Q_k和R_k分别为系统和量测噪声协方差阵；

(二)建立基于直角坐标系的地球卫星轨道运动学方程；

选取历元(J2000.0)地球赤道惯性坐标系，建立基于直角坐标系的地球卫星轨道运动学方程，即状态方程，通过四阶龙格-库塔方法解微分方程计算出地球卫星的位置(x，y，z)和速度(v_x，v_y，v_z)信息，简化的系统状态方程为：

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v_x\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v_y\\ \frac{\mathrm{dz}}{\mathrm{dt}}=v_z\\ \frac{{\mathrm{dv}}_x}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}x}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_x\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}y}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\ΔF}}_y\\ \frac{d{y}_z}{\mathrm{dt}}=-\frac{{\mathrm{\μ}}_{e}z}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-4.5)]+{\mathrm{\ΔF}}_z\end{array}\right.---\left(3\right)$

式中，R_e为地球参考赤道半径，状态矢量X＝[x，y，z，v_x，v_y，v_z]^T，x，y，z，v_x，v_y，v_z分别为卫星在X，Y，Z三个方向的位置和速度， $r=\sqrt{x^2+y^2+z^2}$ 为地心距，μ是地球引力常数，J₂为地球引力二阶带谐项系数，ΔF_x，ΔF_y，ΔF_z为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些摄动力的影响用系统过程噪声w(k)表示；

对于系统过程噪声为零均值白噪声有：

E{w(k)}＝0，E{w(k)w^T(j)}＝Q_kδ_kj，k，j＝1，2，3…；

式中，δ_kj为克朗涅克笛耳塔(Kronecker Delta)函数；

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{cc}1& j=k\\ 0& j\≠k\end{array}\right.;$

(三)  建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方程；

以星敏感器和红外地球敏感器的子系统的量测量为星光角距，星光角距是指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，星光角距a的量测方程为：

$Z_a=\mathrm{\α}+v_a=\arccos (-\frac{\stackrel{\→}{r}\·\stackrel{\→}{s}}{r})+v_a---\left(4\right)$

式中

为卫星在地心惯性坐标系中的位置矢量，由红外地球敏感器获得；

为地球惯性坐标系下的导航星星光方向的单位矢量，由星敏感器识别；r为地心距；v_a为测量误差；

设量测噪声为零均值白噪声：

E{v_a(k)}＝0， $E\left\{v_a\left(k\right)v_a^T\left(j\right)\right\}=R_{k1}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…；

(四)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程；三轴磁强计的测量值为卫星所在位置的地磁场矢量

在卫星本体系中的三个分量，为简化量测模型，取地磁场强度矢量的模作为磁测自主导航观测量，比较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息，量测方程为：

$Z_B=\sqrt{B_x^2+B_y^2+B_z^2}=\sqrt{{\left(\frac{1}{r}\frac{\∂V}{\∂\mathrm{\θ}}\right)}^2+{(-\frac{1}{r\sin \mathrm{\θ}}\frac{\∂V}{\∂\mathrm{\φ}})}^2+{\left(\frac{\∂V}{\∂\stackrel{\→}{r}}\right)}^2}+v_B---\left(5\right)$

其中，B_x、B_y、B_z为地磁场矢量

在卫星本体系中的三个分量；V为地磁场势函数，r为地心距，φ为地理经度，θ为地心余纬，

为地理坐标系下的卫星位置矢量，v_B为量测噪声；

设量测噪声为零均值白噪声：

E{v_B(k)}＝0， $E\left\{v_B\left(k\right)v_B^T\left(j\right)\right\}=R_{k2}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…；

星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息，取测量模型为：

其中，R_e为地球参考赤道半径，为地心纬度，f为地球参考椭球的扁率；v_H(t)为量测噪声；

设量测噪声为零均值白噪声：

E{v_H(k)}＝0， $E\left\{v_H\left(k\right)v_H^T\left(j\right)\right\}=R_{k3}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…；

从式5和式6看出，需建立地心惯性坐标系和地球固联坐标系之间的转换矩阵，从地心赤道惯性坐标系S_i到地球固连坐标系S_e的坐标变换矩阵为：

$L_{\mathrm{ei}}=L_z\left(a_G\right)=\begin{array}{}\end{array}\left[\begin{array}{ccc}{\cos a}_G& {\sin a}_G& 0\\ -{\sin a}_G& {\cos a}_G& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

式中：a_G为Greenwich赤经；

(五)建立以紫外敏感器的输出值为量测量的子系统量测方程；

紫外敏感器的量测值为卫星的位置矢量，紫外敏感器工作在紫外波段，同时观测多个天体目标，提供三轴的航天器姿态信息，定姿精度可达0.05°；另外，通过对地球圆盘的图像处理，提取星体坐标系中的地心方向和地心距离信息；结合定姿过程得到的姿态信息，计算出卫星在惯性系中的位置矢量，子系统的量测方程为：

$Z_u=\left[\begin{array}{c}\stackrel{\→}{r}\\ \stackrel{\‾}{r}\\ r\end{array}\right]+v_u---\left(8\right)$

其中，

为卫星在地心惯性坐标系中的位置矢量， $r=\sqrt{x^2+y^2+z^2}$ 为地心距；v_u为测量误差；

设量测噪声为零均值白噪声：

E{v_u(k)}＝0， $E\left\{v_u\left(k\right)v_u^T\left(j\right)\right\}=R_{k4}{\mathrm{\δ}}_{\mathrm{kj}},$ k，j＝1，2，3…；

(六)  依照最小偏度采样策略选择Sigma采样点；

①选择0≤W₀＜1，Sigma权值为：

$W_i=\left\{\begin{array}{cc}\frac{1-W_0}{2^n}& i=\mathrm{1,2}\\ 2^{i-2}{W}_1& i=3,\·\·\·,n+1\end{array}---\left(9\right)\right.$

$W_i^m=W_i^c=W_i$

式中，W为权值，n为状态方程维数，W_i ^m为均值加权值，W_i ^c为协方差加权值；

②对应状态为1维情况，迭代初始向量：

${\mathrm{\χ}}_0^1=\left[0\right],$ ${\mathrm{\χ}}_1^1=[-\frac{1}{\sqrt{{2W}_1}}],$ ${\mathrm{\χ}}_2^1=\left[\frac{1}{\sqrt{{2W}_1}}\right]---\left(10\right)$

③对于输入维数j＝2，…n时，迭代公式为：

${\mathrm{\χ}}_i^{j+1}=\left\{\begin{array}{cc}\left[\begin{array}{c}{\mathrm{\χ}}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{\mathrm{\χ}}_i^{j-1}\\ -\frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=1,\·\·\·j\\ \left[\begin{array}{c}{0}_{j-1}\\ \frac{1}{\sqrt{{2W}_{j+1}}}\end{array}\right]& i=j+1\end{array}\right.---\left(11\right)$

(七)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程，各子系统分别独立进行Sigma采样点计算以及预测更新和量测更新；

①预测方程：

对所生成的Sigma点加入状态矢量X的均值和协方差信息：

${\mathrm{\χ}}_i=\stackrel{\‾}{X}+\sqrt{P\left(k\right)}{\mathrm{\χ}}_i^j,$ i＝0，1，2，…，n+1                          (12)

χ_i(k+1|k)＝f[χ_i(k|k)]                    (13)

$\hat{X}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{\mathrm{\χ}}_i(k+1|k)---\left(14\right)$

其中L为采样点个数，

为t_k时刻对t_k+1的预测估值，P(k)为误差协方差阵初值，f(·)为系统状态方程；

$P(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k))}^T+Q_k---\left(15\right)$

Z_i(k+1|k)＝h[χ_i(k+1|k)]                      (16)

$\hat{Z}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{Z}_i(k+1|k)---\left(17\right)$

式中，P(k+1|k)为预测估值误差协方差阵，h(·)为量测方程，

为对t_k+1观测值的预测值；

$P_{\mathrm{yy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c(Z_i(k+1|k)-\hat{Z}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T+R_k---\left(18\right)$

$P_{\mathrm{xy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c({\mathrm{\χ}}_i(k+1|k)-\hat{X}(k+1|k)){(Z_i(k+1|k)-\hat{Z}(k+1|k))}^T---\left(19\right)$

②更新方程：

$W(k+1)=P_{\mathrm{xy}}(k+1|k)P_{\mathrm{yy}}^{-1}(k+1|k)---\left(20\right)$

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)(Z(k+1)-\hat{Z}(k+1|k))---\left(21\right)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)P_yy(k+1|k)W^T(k+1)    (22)

式中，W(k+1)为增益矩阵，

为滤波值，P(k+1|k+1)为滤波值误差协方差阵；

(八)利用残差检验法，根据预测滤波残差判断各子滤波器输出值是否有效，如出现故障则将其隔离，否则将滤波结果输入主滤波器进行信息融合；残差检验法的故障函数为：

其中， ${\mathrm{\χ}}_m^2\left(k\right)\stackrel{\mathrm{\Δ}}{=}{v}_k^T\left(k\right)P_{\mathrm{yy}}^{-1}\left(k\right|k-1)v_k\left(k\right)~{\mathrm{\χ}}_m^2,$ 即χ_m ²(k)为具有m个自由度的χ²变量，m为量测矢量Z的维数；v_k为残差， $v_k=z\left(k\right)-\hat{z}\left(k\right|k-1);$

对于正常滤波，取误警率 $P\{{\mathrm{\χ}}_m^2\left(k\right)>T_D\}={\∫}_{{\mathrm{\χ}}_a^2}^{+\∞}{k}_m\left({\mathrm{\χ}}^2\right)d{\mathrm{\χ}}^2=a;$ 其中k_m(·)为χ_m ²的概率密度函数，a为显著性水平，T_D为由误警率确定的门限值；

(九)建立基于UKF算法的无复位联邦UKF滤波方程；

联邦滤波器的信息融合方式采用无复位模式，仅对状态量进行融合和反馈，各子系统的协方差阵独立进行时间更新和预测更新，不参加主滤波器的融合：

$P_{k+1|k+1}^g={\left[\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}\right]}^{-1}$ l＝1，2，3    (24)

${\hat{x}}_{k+1|k+1}^g=P_{k+1|k+1}^g\left[\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\left(P_{k+1|k+1}^i\right)}^{-1}{\hat{x}}_{k+1|k+1}^i\right]$ l＝1，2，3    (25)

(十)按照上述步骤(一)-(九)，输出为地球卫星状态矢量估计值

及其方差矩阵P^g，其中状态估计值

包括地球卫星位置和速度矢量[x，y，z，v_x，v_y，v_z]^T，状态估计方差矩阵P^g包括地球卫星位置和速度估计方差[P_x ^g，P_y ^g，P_z ^g，P_vx ^g，P_vy ^g，P_vz ^g]^T。

Images