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)$
状态矢量X0＝[x0，y0，z0，vx0，vy0，vz0]T，x0，y0，z0，vx0，vy0，vz0，分别为卫星在X，Y，Z三个方 向的初始位置和速度；并设系统过程噪声w(k)和量测噪声v(k)为加性噪声，Qk和 Rk分别为系统和量测噪声协方差阵；
(二)建立基于直角坐标系的地球卫星轨道运动学方程；
选取历元(J2000.0)地球赤道惯性坐标系，建立基于直角坐标系的地球卫 星轨道运动学方程，即状态方程，通过四阶龙格-库塔方法解微分方程计算出地 球卫星的位置(x，y，z)和速度(vx，vy，vz)信息，简化的系统状态方程为：
$\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{{\mu }_{e}x}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\Delta F}}_x\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}=-\frac{{\mu }_{e}y}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]+{\mathrm{\Delta F}}_y\\ \frac{d{y}_z}{\mathrm{dt}}=-\frac{{\mu }_{e}z}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-4.5)]+{\mathrm{\Delta F}}_z\end{array}\right.---\left(3\right)$
式中，Re为地球参考赤道半径，状态矢量X＝[x，y，z，vx，vy，vz]T，x，y，z，vx，vy，vz分别 为卫星在X，Y，Z三个方向的位置和速度，$r=\sqrt{x^2+y^2+z^2}$为地心距，μ是地球引力常 数，J2为地球引力二阶带谐项系数，ΔFx，ΔFy，ΔFz为地球非球形摄动的高阶摄动项 和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响，在简化模型中这些 摄动力的影响用系统过程噪声w(k)表示；
对于系统过程噪声为零均值白噪声有：
E{w(k)}＝0，E{w(k)wT(j)}＝Qkδkj，k，j＝1，2，3…；
式中，δkj为克朗涅克笛耳塔(Kronecker Delta)函数；
${\delta }_{\mathrm{kj}}=\left\{\begin{array}{cc}1& j=k\\ 0& j\ne k\end{array}\right.;$
(三)  建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测 方程；
以星敏感器和红外地球敏感器的子系统的量测量为星光角距，星光角距是指 从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角，星光角 距a的量测方程为：
$Z_a=\alpha +v_a=\arccos (-\frac{\overrightarrow{r}·\overrightarrow{s}}{r})+v_a---\left(4\right)$
式中为卫星在地心惯性坐标系中的位置矢量，由红外地球敏感器获得；为 地球惯性坐标系下的导航星星光方向的单位矢量，由星敏感器识别；r为地心距； va为测量误差；
设量测噪声为零均值白噪声：
E{va(k)}＝0，$E\left\{v_a\left(k\right)v_a^T\left(j\right)\right\}=R_{k1}{\delta }_{\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{\partial V}{\partial \theta }\right)}^2+{(-\frac{1}{r\sin \theta }\frac{\partial V}{\partial \phi })}^2+{\left(\frac{\partial V}{\partial \overrightarrow{r}}\right)}^2}+v_B---\left(5\right)$
其中，Bx、By、Bz为地磁场矢量在卫星本体系中的三个分量；V为地磁场势 函数，r为地心距，φ为地理经度，θ为地心余纬，为地理坐标系下的卫星位置 矢量，vB为量测噪声；
设量测噪声为零均值白噪声：
E{vB(k)}＝0，$E\left\{v_B\left(k\right)v_B^T\left(j\right)\right\}=R_{k2}{\delta }_{\mathrm{kj}},$k，j＝1，2，3…；
星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息，取测量模 型为：

其中，Re为地球参考赤道半径，为地心纬度，f为地球参考椭球的扁率； vH(t)为量测噪声；
设量测噪声为零均值白噪声：
E{vH(k)}＝0，$E\left\{v_H\left(k\right)v_H^T\left(j\right)\right\}=R_{k3}{\delta }_{\mathrm{kj}},$k，j＝1，2，3…；
从式5和式6看出，需建立地心惯性坐标系和地球固联坐标系之间的转换矩 阵，从地心赤道惯性坐标系Si到地球固连坐标系Se的坐标变换矩阵为：
$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)$
式中：aG为Greenwich赤经；
(五)建立以紫外敏感器的输出值为量测量的子系统量测方程；
紫外敏感器的量测值为卫星的位置矢量，紫外敏感器工作在紫外波段，同时 观测多个天体目标，提供三轴的航天器姿态信息，定姿精度可达0.05°；另外， 通过对地球圆盘的图像处理，提取星体坐标系中的地心方向和地心距离信息；结 合定姿过程得到的姿态信息，计算出卫星在惯性系中的位置矢量，子系统的量测 方程为：
$Z_u=\left[\begin{array}{c}\overrightarrow{r}\\ \bar{r}\\ r\end{array}\right]+v_u---\left(8\right)$
其中，为卫星在地心惯性坐标系中的位置矢量，$r=\sqrt{x^2+y^2+z^2}$为地心距；vu 为测量误差；
设量测噪声为零均值白噪声：
E{vu(k)}＝0，$E\left\{v_u\left(k\right)v_u^T\left(j\right)\right\}=R_{k4}{\delta }_{\mathrm{kj}},$k，j＝1，2，3…；
(六)  依照最小偏度采样策略选择Sigma采样点；
①选择0≤W0＜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为状态方程维数，Wi m为均值加权值，Wi c为协方差加权 值；
②对应状态为1维情况，迭代初始向量：
${\chi }_0^1=\left[0\right],$ ${\chi }_1^1=[-\frac{1}{\sqrt{{2W}_1}}],$ ${\chi }_2^1=\left[\frac{1}{\sqrt{{2W}_1}}\right]---\left(10\right)$
③对于输入维数j＝2，…n时，迭代公式为：
${\chi }_i^{j+1}=\left\{\begin{array}{cc}\left[\begin{array}{c}{\chi }_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{\chi }_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的均值和协方差信息：
${\chi }_i=\bar{X}+\sqrt{P\left(k\right)}{\chi }_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}{\Sigma }}{W}_i^m{\chi }_i(k+1|k)---\left(14\right)$
其中L为采样点个数，为tk时刻对tk+1的预测估值，P(k)为误差协方差 阵初值，f(·)为系统状态方程；
$P(k+1|k)=\underset{i=0}{\overset{L-1}{\Sigma }}{W}_i^c({\chi }_i(k+1|k)-\hat{X}(k+1|k)){({\chi }_i(k+1|k)-\hat{X}(k+1|k))}^T+Q_k---\left(15\right)$
Zi(k+1|k)＝h[χi(k+1|k)]                      (16)
$\hat{Z}(k+1|k)=\underset{i=0}{\overset{L-1}{\Sigma }}{W}_i^m{Z}_i(k+1|k)---\left(17\right)$
式中，P(k+1|k)为预测估值误差协方差阵，h(·)为量测方程，为对tk+1 观测值的预测值；
$P_{\mathrm{yy}}(k+1|k)=\underset{i=0}{\overset{L-1}{\Sigma }}{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}{\Sigma }}{W}_i^c({\chi }_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)Pyy(k+1|k)WT(k+1)    (22)
式中，W(k+1)为增益矩阵，为滤波值，P(k+1|k+1)为滤波值误差 协方差阵；
(八)利用残差检验法，根据预测滤波残差判断各子滤波器输出值是否有 效，如出现故障则将其隔离，否则将滤波结果输入主滤波器进行信息融合； 残差检验法的故障函数为：

其中，${\chi }_m^2\left(k\right)\stackrel{\Delta }{=}{v}_k^T\left(k\right)P_{\mathrm{yy}}^{-1}\left(k\right|k-1)v_k\left(k\right)~{\chi }_m^2,$即χm 2(k)为具有m个自由度的χ2变量，m 为量测矢量Z的维数；vk为残差，$v_k=z\left(k\right)-\hat{z}\left(k\right|k-1);$
对于正常滤波，取误警率$P\{{\chi }_m^2\left(k\right)>T_D\}={\int }_{{\chi }_a^2}^{+\infty }{k}_m\left({\chi }^2\right)d{\chi }^2=a;$其中km(·)为χm 2的概率 密度函数，a为显著性水平，TD为由误警率确定的门限值；
(九)建立基于UKF算法的无复位联邦UKF滤波方程；
联邦滤波器的信息融合方式采用无复位模式，仅对状态量进行融合和反馈， 各子系统的协方差阵独立进行时间更新和预测更新，不参加主滤波器的融合：
$P_{k+1|k+1}^g={\left[\underset{i=1}{\overset{l}{\Sigma }}{\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}{\Sigma }}{\left(P_{k+1|k+1}^i\right)}^{-1}{\hat{x}}_{k+1|k+1}^i\right]$l＝1，2，3    (25)
(十)按照上述步骤(一)-(九)，输出为地球卫星状态矢量估计值及其 方差矩阵Pg，其中状态估计值包括地球卫星位置和速度矢量[x，y，z，vx，vy，vz]T， 状态估计方差矩阵Pg包括地球卫星位置和速度估计方差[Px g，Py g，Pz g，Pvx g，Pvy g，Pvz g]T。