CN103528587B - 自主组合导航*** - Google Patents

Abstract

本发明涉及一种自主组合导航***，属于导航***技术领域。将SINS作为主导航***，SAR、CNS作为辅助导航***，构成SINS／SAR／CNS组合导航***，步骤如下：首先设计SINS／SAR和SINS／CNS导航子滤波器，计算获得组合导航***状态的两组局部最优估计值和局部最优误差协方差阵再采用联邦滤波技术将两组局部最优估计值送入主滤波器进行融合得到全局最优估计值和全局最优误差协方差阵最后利用全局最优估计值

Description

自主组合导航***

技术领域

本发明涉及一种自主组合导航***，属于导航***技术领域。

背景技术

自主导航是指在不依赖地面人员测控的条件下，能够自主的精确确定飞行器的位置和速度。也就是说当飞行器处于一个未知的、复杂的、动态的非结构环境中，在没有人干预的条件下，通过环境感知，能够到达期望的目的地，同时能尽量减少时间或能量的消耗等。自主导航传感器具备4个特点：自主、实时、无源和不依靠地面信息。

自主导航是面向我国军用飞行器(军事卫星、军用飞机和巡航弹等)发展实际提出的新的、具有强烈挑战性的研究课题。军用飞行器导航技术向着自主组合导航的方向发展是一个必然趋势。

自主导航技术包括惯性导航、天文导航、无线电导航、卫星导航、地磁导航、地形／图像以及各种视觉信息导航。惯性导航***是利用加速度计等惯性元件敏感运载器在运动过程中的加速度，然后通过积分计算，得到载体的位置与速度等导航参数。惯性导航***的优点在于完全自主式，保密性强，不易受外界条件和人为因素的干扰，全天候，不受天气限制，但最大缺点是误差会随着时间的推移而积累。天文导航***利用光学或射电望远镜接收星体发射来的电磁波去跟踪星体，姿态测量精度高，对于空气稀薄的高空和空间飞行器是较为理想的导航方式。无线电导航***利用无线电技术测量导航参数，包括多普勒效应测速、用雷达测距和测方位、用导航台定位等，其输出信息主要是载***置，对精确导航***来讲，其定位精度不高，且工作范围受到地面台覆盖区域的限制。卫星导航***是天文导航与无线电导航的结合物，典型卫星导航***包括美国的GPS***、欧洲的“伽利略”***、以及我国的“北斗”***，具有导航精度高、使用简单等优点，但是定位精度严重依赖可见星的数量和几何分布，对于观测环境较差区域，因可见星数量少导致精度大大下降，且信号易被干扰或遮蔽。地磁导航技术作为一种无源自主导航方法，具有抗干扰能力强、无积累误差等优点，缺点是精度较差，更适合作为巡航导弹、水面舰船、水中潜器等辅助导航方法。

显然，现有任何单一导航***都难以完全实现高精度自主导航。高精度的自主导航需要由多个机载传感器构成的组合导航***来完成，其实现的关键技术是多传感器信息融合。多传感器信息融合作为新近崛起的一个前沿性的、前景十分广阔的研究领域，广泛地应用于多源信息的综合处理过程。它综合利用了多种类型传感器的不同特点，可以 多方位全面获取目标不同属性信息，提高了自主导航***在时间上和空间上的覆盖范围，提高了导航传感器信息的使用效率并增加了信息的可信度。特别是各种滤波算法的出现，为组合导航***提供了理论基础和数学工具。

目前，工程上组合导***采用的滤波方法主要是卡尔曼滤波(Kalman Filter，KF)和扩展卡尔曼滤波(Extended Kalman Filter，EKF)。KF要求***数学模型必须为线性，当组合导航***模型具有非线性特性时，仍然采用线性模型描述组合导航***及使用KF进行滤波，将会引起线性模型近似误差。

尽管EKF在组合导航***非线性滤波中得到了广泛应用，但它仍然具有理论局限性，具体表现在：(1)当***非线性度较严重时，忽略Taylor展开式的高阶项将引起线性化误差增大，导致EKF的滤波误差增大甚至发散；(2)雅可比矩阵的求取复杂、计算量大，在实际应用中很难实施，有时甚至很难得到非线性函数的雅可比矩阵；(3)EKF将状态方程中的模型误差作为过程噪声来处理，且假设为高斯白噪声，这与组合导航***的实际噪声情况并不相符；同时，EKF是以KF为基础推导得到的，其对***初始状态的统计特性要求严格。因此EKF关于***模型不确定性的鲁棒性很差。

近期所出现的模型预测滤波(MPF)、粒子滤波(PF)及无迹卡尔曼滤波(UKF)等非线性滤波方法以及交互式多模型算法，在处理组合导航***中非线性滤波问题及克服模型不确定等方面，都有各自独到的优势。虽然它们在组合导航***应用方面取得了一定的理论成果，但存在以下问题值得关注：(1)重要性函数的选择直接影响PF的性能高低，开展PF重要性函数选择的通用准则研究极具意义；目前针对许多实际问题，出现了许多选择重要性函数的改进算法，但应用于组合导航***中的重要性函数选择方法较少，有待进一步理论分析；(2)经典的重采样方法虽能有效克服粒子匮乏，但计算量随粒子数增加而成级数递增，***实时性变差，如何解决粒子滤波在组合导航***中的可实现性成为主要问题；(3)目前还没有任何数学理论对PF在何种条件能够收敛的问题做出回答，因此评价和分析PF在组合导航***应用中的性能也就变得十分困难。

对于UT变换及UKF，目前虽然可以得到UT变换的精度证明，但UKF算法尚不能像EKF那样给出稳定性分析，影响了其在组合导航***中的应用。UKF的采样策略已有很多种，其要么精度低，无法精确得到非线性***高阶项信息，对非高斯***的滤波效果不佳。要么精度高，但计算过于复杂而实时性差，造成UKF算法在组合导航***中实现困难。

由此可见，现有的滤波技术不能完全满足高精度自主导航的要求，需要对自主导航高精度非线性算法与数据实时处理技术进行深入研究，为解决军用飞行器自主导航的基本理论和基础性技术难题寻求新途径，以进一步提高军用飞行器的作战能力。

发明内容

本发明的目的在于一种基于全面最优校正的SINS／SAR／CNS自主组合导航***，利用高精度滤波技术和多源信息融合技术对惯性导航***、合成孔径雷达和天文导航***输出的姿态、位置信息进行信息处理和融合，进而对导航误差进行全面最优地估计、校正，以提高组合导航***的精度。

为实现上述目的，本发明技术方案如下：

一种自主组合导航***，在SINS／SAR／CNS组合导航***设计中，由于SINS可以全天候、全天时的提供三维姿态、速度和位置信息，且隐蔽性好，抗干扰能力强，因此，将SINS作为主导航***，SAR、CNS作为辅助导航***，构成SINS／SAR／CNS组合导航***。其步骤为：

首先，设计SINS／SAR和SINS／CNS导航子滤波器，通过计算获得组合导航***状态的两组局部最优估计值和局部最优误差协方差阵然后，采用联邦滤波技术，将两组局部最优估计值送入主滤波器中进行信息融合，得到***状态的全局最优估计值和全局最优误差协方差阵；最后，利用获得的状态的全局最优估计值对捷联惯导***的误差进行实时校正。由于CNS和SAR都不能输出载体的高度信息，因此，本***采用气压高度表输出载体的高度信息对SINS高度通道进行校正，以抑制SINS高度通道发散的问题。SINS／SAR／CNS组合导航***最优估计融合算法为

$\left\{\begin{array}{c}{\mathrm{\Σ}}_{\hat{X},g}={({\mathrm{\Σ}}_{\hat{X},1}^{-1}+{\mathrm{\Σ}}_{\hat{X},2}^{-1})}^{-1}\\ {\hat{X}}_g={\mathrm{\Σ}}_{\hat{X},g}({\mathrm{\Σ}}_{\hat{X},1}^{-1}{\hat{X}}_1+{\mathrm{\Σ}}_{\hat{X},2}^{-1}{\hat{X}}_2)\end{array}---(1-1)\right..$

自主组合导航***，设置SINS／SAR／CNS组合导航***数学模型如下：

(1)状态方程：在SINS／SAR／CNS组合导航***中，由于SAR和CNS的定位精度高，误差远小于SINS的误差，且不随时间积累。因此，为了减少***维数，将SAR和CNS的导航误差考虑为高斯白噪声，且不列为组合导航***的状态量，只将SINS的***误差考虑为SINS／SAR／CNS组合导航***的***状态量。

选取东北天(E-N-U)地理坐标系g作为组合导航***的导航坐标系n，根据惯性导航***的误差方程，SINS／SAR／CNS组合导航***的状态x(t)选取为

$x\left(t\right)={[{\left(\mathrm{\δq}\right)}^T,{\left(\mathrm{\δV}\right)}^T,{\left(\mathrm{\δP}\right)}^T,{\mathrm{\ϵ}}^T,{\▿}^T]}^T---\left(1\right)$

式中，δq=[δ_q0、δ_q1、δ_q2、δ_q3]^T为SINS的姿态误差四元数；δV=[δV_E、δV_N、δV_U]^T为SINS东、北、天方向的速度误差；δP=[δL、δλ、δh]^T为SINS的纬度、经度和高度误差；ε=[ε_x、ε_y、ε_x]^T表示陀螺的随机漂移；是加速度计的常值偏置。

根据式(1)可得SINS／SAR／CNS组合导航***的状态方程为

$\stackrel{\·}{x}=f(x,t)+G\left(t\right)w\left(t\right);---\left(2\right)$

式中，f(x，t)为***的状态阵；w(t)=[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T表示***噪声，[w_gx、w_gy、w_gz]为陀螺的白噪声，[w_ax、w_ay、w_az]为加速度计的白噪声；G(t)为***的噪声驱动矩阵，且***状态阵和噪声驱动阵分别为

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\ω}}_{\mathrm{in}}^n-{\mathrm{BC}}_b^c{\mathrm{\ϵ}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^n-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×V^n+C_b^c{\▿}^b\\ \mathrm{M\δV}+\mathrm{N\δL}\\ 0_{3\×1}\\ 0_{3\×1}\end{array}\right]---\left(3\right)$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\×3}\\ 0_{3\×3}& C_b^c\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(4\right)$

(2)量测方程：

①SINS／SAR子***量测方程：合成孔径雷达通过图像匹配，可以获得载体的水平位置信息和航向角信息，捷联惯性导航***通过对陀螺仪和加速度输出的角运动信息和线运动信息进行解算，可以获得载体的姿态、速度和位置信息。由于SAR不能得到载体的高度信息，为了抑制SINS高度通道发散，采用气压高度表对载体的高度信息进行量测。因此，可以将捷联惯导输出的航向角信息和位置信息与合成孔径雷达输出的航向角信息、水平位置信息以及气压高度表输出的载体高度信息的差作为量测量，SINS／SAR组合导航***的量测方程为

z₁(t)=h₁(x，t)+v₁(t) (9)

式中，h₁(x，t)为量测方程的非线性函数，v(t)=[δψ_S，δL_S，δλ_S，δh_e]^T为量测白噪声，均值为零。

②SINS／CNS子***量测方程：在SINS／CNS组合导航子***中，惯性导航***能够得到载体的姿态四元数和位置信息。天文导航***通过星敏感器对天体进行观测可以得到载体的姿态四元数和载体的水平位置信息(经度和纬度)，但是不能获得载体的高度信息，因此需要引入高度气压表对载体的高度信息进行观测，抑制SINS高度通道发散。选取惯性导航***输出的载体姿态四元数和位置信息与天文导航***输出的载体姿态四元数和水平位置信息，以及高度气压表输出的载体高度信息之差作为量测量，SINS／CNS组合导航子***的量测方程为

z₂(t)=h₂(t)x(t)+v，₂(t) (11)

式中，h₂(t)为量测矩阵，v₂(t)=[δq_C0，δq_C1,δq_C2，δq_C3，δL_C，δλ_C，δh_e]^T为量测白噪声，均值为零。

(3)自主导航高精度非线性滤波算法：设计了一套适合于SINS／CNS／SAR自主组合导航***的高精度、非线性滤波算法，该套算法包括：

①抗差自适应Unscented粒子滤波算法；②渐消自适应Unscented粒子滤波；③模糊抗差自适应粒子滤波；④自适应SVD-UKF滤波算法；⑤自适应平方根Unscented粒子滤波。具体如下：

①抗差自适应Unscented粒子滤波算法：

抗差自适应Unscented粒子滤波，充分吸收了抗差估计、抗差自适应滤波和粒子滤波的优点，通过等价权因子和自适应因子将抗差估计原理与UPF结合起来，选择适当的权函数和自适应因子控制状态模型信息和量测模型信息，抑制异常干扰的影响。

抗差自适应Unscented粒子滤波算法的步骤如下：

(a)初始化，根据初始均值和均方差抽取N个粒子，在k=0时刻，i＝1，2，…，N，设置权值为

(b)在k=1，2，…，N时刻，按照下面次序计算：

(b1)计算等价权和自适应因子α。选用IGG方案构造等价权函数，IGG法属于降权函数，即对量测值做抗差限制，若取其倒数，则定义为方差膨胀因子函数。

设等价权矩阵为 $\stackrel{\‾}{P}=\mathrm{diag}({\stackrel{\‾}{P}}_1,{\stackrel{\‾}{P}}_2,...,{\stackrel{\‾}{P}}_k),$

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.---\left(12\right)$

根据需要也可以采用另一种表达式

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(13\right)$

其中k₀∈(1，1.5)，k₁∈(3，8)，V_k为观测值l_k的残差向量， 为当前时刻状态参数估计值。自适应因子选取如下

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.---\left(14\right)$

其中c₀∈(1，1.5)，c₁∈(3，8)，tr(·)表示矩阵的迹，为预报值，即 可以看出权函数和自适应因子构造形式基本相同，二者都是重要的调节因子。前者通过对残差的判断来选取，而后者根据状态估计值与预报值之差来选取。

(b2)计算Sigma点，由UKF算法更新粒子得到且满足设新样本为2N+1个Sigma点采样点为

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(15\right)$

式中λ=α²(n+v)表示尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度，β一般根据先验知识来取值(对于高斯分布最佳取值为(b2)，W_j表示第j个Sigma点的权值，满足∑W_j=1，j=0，1，…2N。

(c)计算权值 $w_k^i=w_{k-1}^i\frac{p\left(y_k\right|{\stackrel{\&OverBar;}{x}}_k^{i^{*}})p\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}})}{q\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}},l_k)},$ 并归一化为 ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

(d)计算估计式 ${\hat{N}}_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\tilde{w}}_k^i\right)}^2.$

将所得结果与既定阙值进行比较，判断粒子退化的严重程度，越小，表明退化越严重。在这种情况下，可对上面获得的后验密度进行重采样，重新得到M个新的粒子，并赋予各个粒子相同权值1／M。

(e)计算非线性状态量估计值。重复上面步骤(b)。

上述步骤在选取重要性密度函数时，利用了两个重要的调节因子，即等价权因子和自适应因子。通过两者对UT变换后所得粒子采样点更合理的分配有用信息，为重要性采样过程提供更好的采样分布函数。

②渐消自适应Unscented粒子滤波：

改进的渐消自适应Unscented粒子滤波是以粒子滤波为基本框架，融合渐消自适应滤波原理和UT变换过程，吸收各单一算法优点，建立参数可自适应调节的重要性密度分布函数，充分高效的利用最新量测信息，使其更接近真实的分布函数，从而使滤波算法具有更好的自适应性和鲁棒性。

Unscented粒子滤波算法主要利用UT变换得到采样点，实现对状态向量后验分布的近似。与蒙特卡罗方法不同，Unscented粒子滤波不是随机的从给定分布中进行采样，是取少数确定的Sigma点作为采样点。Sigma采样点为

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(20\right)$

式中λ=α²(N+v)代表尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度。对于不同的***模型和噪声假设，UT变换算法具有不同的形式，决定UT变换算法表达式的关键是确定Sigma点采样策略，即Sigma点个数、位置和权值。

渐消自适应Unscented粒子滤波算法采用渐消因子限制滤波器的记忆长度，充分利用当前观测值不断修正预测值，并对未知的或不确切的***模型参数、噪声统计参数等进行估计和修正。算法主要步骤为：

(a)初始化，k=0时，其中k表示时刻。统一设置权值为其中k表示时刻，N表示粒子个数。

(b)计算Sigma点，设新样本为计算2N+1个Sigma采样点，利用UKF算法对粒子进行预测和更新，

其中各符号意义同上，下式中β一般根据先验知识来取值(对于高斯分布最佳取值为2)，W_j表示第j个Sigma点的权值，满足∑W_j=1，j=0，1，…2N，并进行时间更新和测量更新。利用渐消自适应扩展卡尔曼滤波思想计算渐消因子，利用式α_k，并计算

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i)---\left(31\right)$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T---\left(32\right)$

得到作为粒子采样的重要性密度函数，进行重要性采样。

(c)从重要性密度函中进行采样后，并对每个粒子计算权值

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(33\right)$

并计算归一化权值。

${\hat{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i---\left(34\right)$

(d)利用估计式判断粒子退化程度是否严重，然后从上面获得的后验密度进行重采样重新得到M个新的粒子，重新赋予各个粒子相同权值

(e)计算非线性状态量估计值。

${\tilde{x}}_k=\underset{i=0}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i{x}_k^{i**}---\left(35\right)$

返回第(2)步，按新观测量递归计算下一时刻的状态估计值。

③模糊抗差自适应粒子滤波：

(I)基于模糊理论构造等价权：为了得到既有较强抗差性又有较高自适应性的估值，可根据数据残差将权划分为：保权区(保持原观测值不变)、降权区(对观测值作抗差限制)以及拒绝区(权为零)。设计一维模糊控制器，构造权因子γ的步骤如下：

(a)模糊化。模糊化的作用是将输入的精确量转换成模糊化量，即将输入量(精确值)模糊化变为模糊变量(其中为标准化残差)，将确定的输入转化为由隶属度描述的模糊集。

具体过程：将输入变量的模糊子集划分为{太大，较大，正常}，简记为{B_e，B_c，B_n}，输入量化后，的论域为X＝{0，1，2，3，4}；输出γ的模糊子集为{极小，较小，正常}，简记为{S_e，S_c，S_n}，其论域为Y大小分为5个等级，以表示不同的值，即Y＝{0，1，2，3，4}。分别对输入和输出γ进行模糊量化。

(b)根据人的直觉思维推理和实践经验，由输入量和输出量权因子γ的关系，设计模糊控制规则。如果太大，则γ极小；如果较大，则γ较小；如果正常，则γ正常。

根据模糊规则可以确定模糊关系为

R=(B_e×S_e)+(B_c×S_c)+(B_n×S_n)

其中，“×”表示模糊向量的笛卡尔乘积。经计算

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right]$

(c)根据模糊控制原理，由输入变量的模糊子集和模糊关系矩阵R通过模糊推理得到权因子为γ的一个模糊集合，得出最终的模糊控制量。

(d)将该模糊控制量进行解模糊得到精确的输出控制量，即权因子γ。模糊推理结果转化为精确值的过程称为反模糊化。在反模糊化处理过程时，采用最大隶属度原则。

(II)模糊抗差自适应粒子滤波算法

模糊抗差自适应粒子滤波(Fuzzy Robust Adaptively Particle Filter，FRAPF)算法的步骤如下：

(a)初始化。在k=0时刻，根据重点密度抽样出N个粒子，假定抽样出的每个粒子用表示，令k=1；

(b)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。以预测残差为变量构造状态模型误差的判别统计量为

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(40\right)$

其中，为k时刻第i个状态预测信息，通过求出；表示预测残差的协方差矩阵；tr(·)为矩阵求迹算子。基于预测残差判别统计量的自适应因子函数为

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0<c<2.5。

(c)更新：

$x_k^i=f(x_{k-1}^i,v_{k-1})---\left(42\right)$

由根据式(42)更新k时刻的粒子

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)})---\left(43\right)$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})}---\left(44\right)$

更新权值和归一化权值，i＝1，2，…，N。

(d)重采样：对所有粒子的权值按照降序进行排序，设置门限样本点数为N_th(通常可选为N／2或N／3)，有效样本点数为N_eff，当N_eff<N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)滤波

$p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i)---\left(45\right)$

计算k时刻的滤波密度并进行再采样，令k＝k+1，返回(b)。

抗差自适应滤波的自适应因子α_k既不单独处理模型误差协方差矩阵也不单独处理前一历元状态参数向量估计的协方差矩阵而是作用于整体的状态参数预报向量的等价协方差矩阵由于抗差自适应滤波对观测信息采用了抗差估计原理，当观测存在异常时，将动力模型信息作为一个整体，采用统一的自适应因子调整动力学模型信息对状态参数的整体贡献，从而得到可靠的滤波结果。

④自适应SVD-UKF滤波算法：

(I)奇异值分解：奇异值分解(SVD)是数值代数计算中稳定性和精度较好的一种矩阵分解方法，易于在计算机上实现。定义如下。

假定A∈R^m×n(m≥n)，则矩阵A的奇异值分解可表示为

$A={\mathrm{UAV}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T---\left(46\right)$

式(46)中，U∈R^m×m，Λ∈R^m×n，V∈R^n×n，S=diag(s₁,s₂，...，s_r)。s₁≥s₂≥…≥s_r≥0称为矩阵A的奇异值，U和V的列向量分别称为矩阵A的左、右奇异向量。

如果A^TA正定，则式(46)可简化为

$A=U\left[\begin{array}{c}S\\ 0\end{array}\right]V^T---\left(47\right)$

若A对称正定，则A＝USU^T，此时左奇异向量与右奇异向量相等，从而可以减少计算量。

(II)统计量及自适应因子的确定：以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。预测残差(新息)含有未经观测信息修正的状态，更能反映动态***受到的扰动。预测残差表示为

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{x}}_k\right)-y_k---\left(48\right)$

式(48)中，为k时刻状态预测信息。用预测残差构造状态模型误差判别统计量如下。

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

式(49)中，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子。

自适应因子函数选为

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.---\left(50\right)$

式(50)中，α_k表示自适应因子，c为经验常数，通常情况下1<c<2.5。

(III)自适应SVD-UKF算法步骤：

自适应SVD-UKF算法步骤如下：

(a)初始化：对状态方程中的参数初始化，并计算Sigma点均值和协方差的权重系数w^(m)、w^(c)。

(b)奇异值分解、计算Sigma点向量：

(c)时间更新：

(d)量测更新；

⑤自适应平方根Unscented粒子滤波

设***动力学方程为

X_k=Φ_k,k-1X_k-1+W_k (67)

式中，X_k是k时刻n维状态向量，Φ_k，k-1为n×n状态转移矩阵，W_k为***噪声向量，其协方差阵为

观测方程为

Y_k=H_kX_k+e_k (68)

式中，Y_k是k时刻m维观测向量，H_k为m×n维设计矩阵，e_k为观测噪声向量，其协方差阵为∑_k。W_k，W_j，e_k，e_j(j≠k)互不相关。

已知状态的初始概率密度函数为p(X₀|Y₀)=p(X₀)，则根据贝叶斯估计理论，非线性时变***的状态预测方程和状态更新方程分别为

p(X_k|Y_1：k-1)=∫p(X_k|X_k-1)p(X_k-1|Y_1：k-1)dX_k-1 (69)

$p\left(X_k\right|Y_{1:k})=\frac{p\left(Y_k\right|X_k)p\left(X_k\right|Y_{1:k-1})}{p\left(Y_k\right|Y_{1:k-1})}---\left(70\right)$

式中，p(X_k|X_k-1)为状态转移密度，p(X_k-1|Y_1：k-1)为k-1时刻的后验密度；p(X_k|Y_1：k-1)为先验分布，p(Y_k|X_k)为似然密度，p(Y_k|Y_1：k-1)为归一化常数，可由下式求得

p(Y_k|Y_1：k-1)=∫p(Y_k|X_k)p(X_k|Y_1：k-1)dX_k (71)

式(69)～(71)构成了递推贝叶斯估计。式(71)仅对某些动态***能获得解析解。基于随机采样的Monte Carlo方法可将积分运算转化为有限样本点的求和运算，即可得后验概率密度函数的近似表达形式。实际中的后验密度p(X_k|Y_1：k)可能是多变量、非标准概率分布，需要借助重要性采样算法对其采样，因而需要构造重要性函数。选择恰当的重要性函数能有效解决粒子滤波的粒子退化问题。

本项目采用UKF算法产生粒子滤波的重要性密度函数，该算法充分利用最新的观测数据，实时修正动力学模型和噪声统计参数引起的误差。自适应平方根UPF的步骤如下。

(a)初始化(k=0)：随机抽取N个初始粒子(i＝1，2，…，N)。假设 ${\hat{X}}_0^i=E\left[X_0^i\right],S_0^i=\mathrm{chol}\left\{E\right[(X_0^i-{\hat{X}}_0^i){(X_0^i-{\hat{X}}_0^i)}^T\left]\right\},w_0^i=p\left(Y_0\right|X_0^i).$ 其中，和分别表示初始时刻第i个粒子及其估计值，表示初始时刻第i个Cholesky分解因子，表示第i个粒子的初始化权值，chol{·}表示矩阵的Cholesky分解算子。

(b)采用自适应平方根UKF滤波算法更新每个粒子得到 表示k时刻第i个粒子的协方差。

(b1)计算Sigma点和权值；

(b2)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。

预测残差(或新息)含有未经观测信息修正的状态，更能反映动态***的扰动。k时刻第i个预测残差向量表示为

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(74\right)$

以预测残差为变量构造状态模型误差的判别统计量为

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(75\right)$

其中，为k时刻第i个状态预测信息，通过求出，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子。

基于预测残差判别统计量的自适应因子函数为

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(76\right)$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0<c<2.5。

(b3)时间更新(状态预测)；

(b4)测量更新(状态估计)；

该步骤中使用了线性代数中的QR分解、Cholesky分解，以Cholesky分解因子的形式直接传播和更新状态协方差矩阵，从而增强了状态协方差矩阵更新过程中的数值稳定性，保证了协方差矩阵的正定性。其中，qr{·}表示矩阵的QR分解。

(c)重要性采样权值计算：令重要性分布函数采样粒子N(·)表示正态分布。分别通过 $w_k^i=w_{k-1}^i\frac{p(Y_k/X_k^i)p(X_k^i/X_{k-1}^i)}{q\left({\stackrel{\&OverBar;}{X}}_k^i\right|{\stackrel{\&OverBar;}{X}}_{0:k-1}^i,Y_{1:k})},$ 更新权值和归一化权值，i＝1，…，N。

(d)重采样：设置门限样本点数为N_th(通常可选为N/2或N/3)，有效样本点数为N_eff，当N_eff<N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)状态更新：

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{X}}_k^i{\tilde{w}}_k^i---\left(93\right)$

${\widehat{\mathrm{\&Sigma;}}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i({\hat{X}}_k-{\tilde{X}}_k^i){({\hat{X}}_k-{\tilde{X}}_k^i)}^T---\left(94\right)$

在SINS／SAR导航子***中，捷联惯性导航***利用陀螺和加速度计对载体的线运动和角运动进行测量，获得载体的角增量和比力增量，然后利用三维姿态更新算法、速度更新算法和位置更新算法进行导航计算，可以获得载体实时的三维姿态信息、速度信息和位置信息。合成孔径雷达通过距离-多普勒成像原理，可以实时获得目标区域高分辨的SAR图像，然后采用基于特征的图像匹配算法与机载数字地图库中的基准图进行图像匹配，可以得到载体的航向角和水平位置信息。由于合成孔径雷达无法获得载体的高度信息，而捷联惯性导航***的高度通道是发散的，因此采用气压高度表测量载体实时高度，以解决捷联惯导高度通道不可观的问题。采用间接滤波的方法，将捷联惯导***输出的航向角和位置信息与合成孔径雷达输出的航向角和水平位置信息以及气压高度表输出的高度信息的差值作为量测量，然后送入滤波器中进行非线性滤波，获得组合导航***状态的最优估计值，再利用该估计值对捷联惯性导航***的误差进行修正。同时利用修正后的SINS导航信息对合成孔径雷达进行运动补偿，以提高SAR的成像质量。

在SINS／CNS组合导航***设计中，惯性器件可以输出载体角运动信息和线运动信息，利用捷联惯导的导航解算算法对这些信息进行解算，可以获得载体实时的三维姿态信息、速度信息和位置信息。天文导航***中的星敏感器可以输出被观测星的赤经、赤纬和旋角，对这些信息进行解算可以得到载体的水平位置信息和姿态信息。由于天文导航***不能获得载体的高度信息，而捷联惯性导航***的高度通道是发散的，因此，采用气压高度表测量载体的高度，对捷联惯性导航***的高度通道进行校正。

由于天文导航***的星敏感器固连在载体上，忽略安装误差，则星敏感器坐标系与载体坐标重合。星敏感器观测到的天体高度角和方位角，通过计算可以获得天体的星光单位方向矢量，利用姿态解算算法可以计算出载体系相对于惯性系的姿态矩阵再根据可求得载体系到导航系的姿态转换矩阵，从而获得天文导航***计算得到的载体系b到导航系n的姿态四元数。星敏感器得到的单位方向矢量通过SINS提供的数学平台基准进行坐标变换，然后利用天文导航***的高度差法，可以计算出载体的经度λ_cns和纬度L_cns信息。将惯性导航***输出的导航信息q_sins，L_sins，λ_sins，h_sins和天文 导航***输出的q_cns，L_cns，λ_cns信息以及气压高度表测得的载体高度信息h_e作差，送到SINS／CNS滤波器中进行滤波计算，可以获得状态的最优估计值。最后，用状态的最优估计值对捷联惯导***的导航参数误差进行校正，使捷联惯导***为天文导航***提供更加精确的数学平台基准。

本发明的有益效果在于：本发明提出一种基于全面最优校正的SINS／SAR／CNS自主组合导航***，能应用于不满足轨道动力学特性的高空长航时无人机，实现了基于星光折射的解析天文定位；利用高精度滤波算法和信息融合技术对惯性导航***、合成孔径雷达和天文导航***输出的姿态、位置信息进行信息处理和融合，进而对导航误差进行全面最优地估计、校正，提高了组合导航***的精度；具有计算量小、可靠性高等优点，并且还可以应用于临近空间飞行器、空天往返飞行器、弹道导弹、变轨航天器等飞行器，具有广阔的应用前景。

附图说明

图1为该发明中SINS／SAR组合导航***原理图。

图2为该发明中SINS／CNS组合导航子***原理。

图3为该发明中SINS／SAR／CNS组合导航原理图。

具体实施方式

下面结合附图进一步阐述该发明的具体实施方式。

实施例

SINS／SAR组合导航原理图如图1所示。图2为该发明中SINS／CNS组合导航子***原理。图3为该发明中SINS／SAR／CNS组合导航原理图。

SINS／SAR／CNS组合导航***数学模型如下：

(1)状态方程：在SINS／SAR／CNS组合导航***中，由于SAR和CNS的定位精度高，误差远小于SINS的误差，且不随时间积累。因此，为了减少***维数，将SAR和CNS的导航误差考虑为高斯白噪声，且不列为组合导航***的状态量，只将SINS的***误差考虑为SINS／SAR／CNS组合导航***的***状态量。

选取东北天(E-N-U)地理坐标系g作为组合导航***的导航坐标系n，根据惯性导航***的误差方程，SINS／SAR／CNS组合导航***的状态x(t)选取为

$x\left(t\right)={[{\left(\mathrm{\&delta;q}\right)}^T,{\left(\mathrm{\&delta;V}\right)}^T,{\left(\mathrm{\&delta;P}\right)}^T,{\mathrm{\&epsiv;}}^T,{\&dtri;}^T]}^T---\left(1\right)$

式中，δq=[δq₀、δq₁、δq₂、δq₃]^T为SINS的姿态误差四元数；δV=[δV_E、δV_N、δV_U]^T为SINS东、北、天方向的速度误差；δP=[δL、δλ、δh]^T为SINS的纬度、经度和高度误差；ε=[ε_x、ε_y、ε_x]^T表示陀螺的随机漂移；是加速度计的常值偏置。

根据式(1)可得SINS／SAR／CNS组合导航***的状态方程为

$\stackrel{\&CenterDot;}{x}=f(x,t)+G\left(t\right)w\left(t\right)---\left(2\right)$

式中，f(x，t)为***的状态阵；w(t)＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T表示***噪声，[w_gx、w_gy、w_gz]为陀螺的白噪声，[w_ax、w_ay、w_az]为加速度计的白噪声；G(t)为***的噪声驱动矩阵，且***状态阵和噪声驱动阵分别为

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\&omega;}}_{\mathrm{in}}^n-{\mathrm{BC}}_b^c{\mathrm{\&epsiv;}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;{\mathrm{\&delta;V}}^n-(2\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&delta;\&omega;}}_{\mathrm{en}}^n)\&times;V^n+C_b^c{\&dtri;}^b\\ \mathrm{M\&delta;V}+\mathrm{N\&delta;L}\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right]---\left(3\right)$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^c\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]---\left(4\right)$

(2)量测方程：

①SINS／SAR子***量测方程：合成孔径雷达通过图像匹配，可以获得载体的水平位置信息和航向角信息，捷联惯性导航***通过对陀螺仪和加速度输出的角运动信息和线运动信息进行解算，可以获得载体的姿态、速度和位置信息。由于SAR不能得到载体的高度信息，为了抑制SINS高度通道发散，采用气压高度表对载体的高度信息进行量测。因此，可以将捷联惯导输出的航向角信息和位置信息与合成孔径雷达输出的航向角信息、水平位置信息以及气压高度表输出的载体高度信息的差作为量测量，则量测量可以表示为

$z_1\left(t\right)=\left[\begin{array}{c}{\mathrm{\&psi;}}_I-{\mathrm{\&psi;}}_S\\ L_I-L_S\\ {\mathrm{\&lambda;}}_I-{\mathrm{\&lambda;}}_S\\ h_I-h_e\end{array}\right]=\left[\begin{array}{c}(\mathrm{\&psi;}+{\mathrm{\&delta;\&psi;}}_I)-(\mathrm{\&psi;}+{\mathrm{\&delta;\&psi;}}_S)\\ (L+{\mathrm{\&delta;L}}_I)-(L+{\mathrm{\&delta;L}}_S)\\ (\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_I)-(\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_S)\\ (h+{\mathrm{\&delta;h}}_I)-(h+{\mathrm{\&delta;h}}_e)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&psi;}}_I\\ {\mathrm{\&delta;L}}_I\\ {\mathrm{\&delta;\&lambda;}}_I\\ {\mathrm{\&delta;h}}_I\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&delta;\&psi;}}_S\\ {\mathrm{\&delta;L}}_S\\ {\mathrm{\&delta;\&lambda;}}_S\\ {\mathrm{\&delta;h}}_e\end{array}\right]---\left(5\right)$

式中，ψ_I、L_I、λ_I和h_I分别为SINS输出的航向角、纬度、经度和高度信息，ψ_S、L_S和λ_S分别为SAR输出的航向角、纬度和经度信息，h_e为气压高度表输出的高度信息，δ·表 示各项对应的误差。

欧拉角形式的姿态误差角(航向误差角δψ_I、俯仰误差角δθ_I和横滚误差角δγ_I)描述的是理想的导航系n(即理想的平台系T)到捷联惯性导航***导航计算机解算出的导航系c(即实际的平台系P)之间的误差。将导航计算机计算出的导航系c通过旋转，可以得到c系到n系的转换矩阵为

$\begin{array}{c}{C}_{c1}^n=\left[\begin{array}{cc}\cos \mathrm{\&delta;\&lambda;}\cos \mathrm{\&delta;\&psi;}+\sin \mathrm{\&delta;\&lambda;}\sin \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}& \sin \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\\ -\cos \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}+\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}& \cos \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\\ -\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&theta;}& \sin \mathrm{\&delta;\&theta;}\end{array}\right.\\ \begin{array}{c}\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}-\cos \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}\\ -\sin \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}-\cos \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}\\ \cos \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\end{array}\end{array}---\left(6\right)$

当采用姿态四元数误差角表示理想导航系n与计算导航系c之间的误差角时，设姿态误差四元数为δq=[δq₀，δq₁，δq₂，δq₃]^T，则用姿态误差四元数表示的c系到n系的转换矩阵为

$C_{c2}^n=\left[\begin{array}{ccc}{\mathrm{\&delta;q}}_0^2+{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2& 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)& 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2)\\ 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)& {\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2+{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2& 2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)\\ 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2)& 2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)& {\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2+{\mathrm{\&delta;q}}_3^2\end{array}\right]---\left(7\right)$

由于矩阵与矩阵都描述的是导航计算机计算得到的导航系c与理想导航系n之间的转换矩阵，因此式(6)与式(7)相等，对等式进行计算可得欧拉角形式的姿态误差角与四元数形式的姿态误差角之间的转换关系为

$\left\{\begin{array}{c}\mathrm{\&delta;\&psi;}=\arctan \left(\frac{2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)}{{\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2+{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2}\right)\\ \mathrm{\&delta;\&theta;}=\arcsin \left(2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)\right)\\ \mathrm{\&delta;\&gamma;}=\arctan \left(\frac{2({\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3)}{{\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2+{\mathrm{\&delta;q}}_3^2}\right)\end{array}\right.---\left(8\right)$

根据式(5)和式(8)，可得SINS／SAR组合导航***的量测方程为

z₁(t)=h₁(x，t)+v₁(t) (9)

式中，h₁(x，t)为量测方程的非线性函数，v(t)=[δψ_S，δL_S，δλ_S，δh_e]^T为量测白噪声，均值为零。

②SINS／CNS子***量测方程：在SINS／CNS组合导航子***中，惯性导航***能够得到载体的姿态四元数和位置信息。天文导航***通过星敏感器对天体进行观测可以得到载体的姿态四元数和载体的水平位置信息(经度和纬度)，但是不能获得载体的高度信息，因此需要引入高度气压表对载体的高度信息进行观测，抑制SINS高度通道发散。选取惯性导航***输出的载体姿态四元数和位置信息与天文导航***输出的载体姿态四元数和水平位置信息，以及高度气压表输出的载体高度信息之差作为量测量，则量测量可以表示为

$z_2\left(t\right)=\left[\begin{array}{cc}{q}_{I0}& q_{C0}\\ q_{I1}& q_{C1}\\ q_{I2}& q_{C2}\\ q_{I3}& -q_{C3}\\ L_I& L_C\\ {\mathrm{\&lambda;}}_I& {\mathrm{\&lambda;}}_C\\ h_I& h_e\end{array}\right]=\left[\begin{array}{c}(q_0+{\mathrm{\&delta;q}}_{I0})-(q_0+{\mathrm{\&delta;q}}_{C0})\\ (q_1+{\mathrm{\&delta;q}}_{I1})-(q_1+{\mathrm{\&delta;q}}_{C1})\\ (q_2+{\mathrm{\&delta;q}}_{I2})-(q_2+{\mathrm{\&delta;q}}_{C2})\\ (q_3+{\mathrm{\&delta;q}}_{I3})-(q_3+{\mathrm{\&delta;q}}_{C3})\\ (L+{\mathrm{\&delta;L}}_1)-(L+{\mathrm{\&delta;L}}_C)\\ (\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_I)-(\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_C)\\ (h+{\mathrm{\&delta;h}}_I)-(h+{\mathrm{\&delta;h}}_e)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;q}}_{I0}\\ {\mathrm{\&delta;q}}_{I1}\\ {\mathrm{\&delta;q}}_{I2}\\ {\mathrm{\&delta;q}}_{I3}\\ {\mathrm{\&delta;L}}_I\\ \mathrm{\&delta;}{\mathrm{\&lambda;}}_I\\ {\mathrm{\&delta;h}}_I\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&delta;q}}_{C0}\\ {\mathrm{\&delta;q}}_{C1}\\ {\mathrm{\&delta;q}}_{C2}\\ {\mathrm{\&delta;q}}_{C3}\\ {\mathrm{\&delta;L}}_C\\ {\mathrm{\&delta;\&lambda;}}_C\\ {\mathrm{\&delta;h}}_e\end{array}\right]---\left(10\right)$

式中，q_I0、q_I1、q_I2、q_I3为惯导***输出的姿态四元数，L_I、λ_I、h_I分别为惯导***输出的纬度、经度和高度信息；q_C0、q_C1、q_C2、q_C3为天文导航***输出的姿态四元数，L_C、λ_C分别为天文导航输出的纬度和经度信息；h_e为气压高度表输出的高度信息；δ·表示各项对应的误差。

根据式(1)和式(10)可得SINS／CNS组合导航子***的量测方程为

z₂(t)=h₂(t)x(t)+v₂(t) (11)

式中，h₂(t)为量测矩阵，v₂(t)=[δq_C0，δq_C1,δq_C2，δq_C3，δL_C，δλ_C，δh_e]^T为量测白噪声，均值为零。

(3)自主导航高精度非线性滤波算法：设计了一套适合于SINS／CNS／SAR自主组合导航***的高精度、非线性滤波算法，该套算法包括：

①抗差自适应Unscented粒子滤波算法；②渐消自适应Unscented粒子滤波；③模糊抗差自适应粒子滤波；④自适应SVD-UKF滤波算法；⑤自适应平方根Unscented粒子滤波。具体如下：

①抗差自适应Unscented粒子滤波算法：

抗差自适应Unscented粒子滤波，充分吸收了抗差估计、抗差自适应滤波和粒子滤波的优点，通过等价权因子和自适应因子将抗差估计原理与UPF结合起来，选择适当的权函数和自适应因子控制状态模型信息和量测模型信息，抑制异常干扰的影响。

抗差自适应Unscented粒子滤波算法的步骤如下：

(a)初始化，根据初始均值和均方差抽取N个粒子，在k=0时刻，i＝1，2，…，N，设置权值为

(b)在k=1，2，…，N时刻，按照下面次序计算：

1)计算等价权和自适应因子α。选用IGG方案构造等价权函数，IGG法属于降权函数，即对量测值做抗差限制，若取其倒数，则定义为方差膨胀因子函数。

设等价权矩阵为 $\stackrel{\&OverBar;}{P}=\mathrm{diag}({\stackrel{\&OverBar;}{P}}_1,{\stackrel{\&OverBar;}{P}}_2,...,{\stackrel{\&OverBar;}{P}}_k),$

$\stackrel{\&OverBar;}{P_k}=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.---\left(12\right)$

根据需要也可以采用另一种表达式

$\stackrel{\&OverBar;}{P_k}=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0{(k_1-|V_k\left|\right)}^2}{\left|V_k\right|{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(13\right)$

其中k₀∈(1，1.5)，k₁∈(3，8)，V_k为观测值l_k的残差向量， 为当前时刻状态参数估计值。自适应因子选取如下

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.---\left(14\right)$

其中c₀∈(1，1.5)，c₁∈(3，8)，tr(·)表示矩阵的迹，为预报值，即 可以看出权函数和自适应因子构造形式基本相同，二者都是重要的调节因子。前者通过对残差的判断来选取，而后者根据状态估计值与预报值之差来选取。

2)计算Sigma点，由UKF算法更新粒子得到且满足设新样本为2N+1个Sigma点采样点为

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(15\right)$

式中λ=α²(n+v)表示尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度，β一般根据先验知识来取值(对于高斯分布最佳取值为2)，W_j表示第j个Sigma点的权值，满足∑W_j=1，j=0，1，…2N。

利用UKF算法对粒子进行预测和更新：

$W_0^m=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})},W_0^c=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})---\left(16\right)$

$W_j^c=W_j^m=W_j=(N+\mathrm{\&lambda;}),j=\mathrm{1,2},...,2N---\left(17\right)$

${\mathrm{\&chi;}}_{k|k-1}^i=f\left({\mathrm{\&chi;}}_{k-1}^i\right)---\left(18\right)$

${\stackrel{\&OverBar;}{x}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{\mathrm{\&chi;}}_{j,k|k-1}^i---\left(19\right)$

$L_{k|k-1}^i=h\left({\mathrm{\&chi;}}_{k|k-1}^i\right)---\left(20\right)$

${\stackrel{\&OverBar;}{l}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{L}_{j,k|k-1}^i---\left(21\right)$

$P_{l_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c[{(L}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]\&CenterDot;{[L_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]}^T+Q_k---\left(22\right)$

$P_{x_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[L_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]}^T+R_k---\left(23\right)$

$P_{x_k{x}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[{\mathrm{\&chi;}}_{j,k|k-1}^i,-x_{k|k-1}^i]}^T---\left(24\right)$

$K_k=P_{x_k{l}_k}{P}_{l_k{l}_k}^{-1}---\left(25\right)$

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(l_k-{\stackrel{\&OverBar;}{L}}_{k|k-1}^i)---\left(26\right)$

$P_k^i=P_{x_k{x}_k}^i-K_k{P}_{l_k{l}_k}{K}_k^T---\left(27\right)$

由式(26)～(27)可以得到

利用等价权和自适应因子计算采样粒子估值和方差

${\stackrel{\&OverBar;}{x}}_k^{i^{*}}={\stackrel{\&OverBar;}{x}}_{k|k-1}^i,+K_k^{*}(l_k-{\stackrel{\&OverBar;}{L}}_{k|k-1}^i)---\left(28\right)$

$P_k^{i^{*}}={\mathrm{\&alpha;}}_k{P}_{x_k{x}_k}^i-K_k^{*}{\stackrel{\&OverBar;}{P}}_{l_k{l}_k}{K}_k^{*T}---\left(29\right)$

$K_k^{*}=P_{x_k{l}_k}{\stackrel{\&OverBar;}{P}}_{l_k{l}_k}^{-1}---\left(30\right)$

上式表明通过自适应因子α_k和可以影响并调节使重要性密度函数更接近实际分布。根据式(16)～(19)得到作为粒子采样的重要性密度函数，再进行重要性采样。从式(18)可以看出，当量测模型存在异常时，等价权矩阵元素减小，参数估值时利用量测信息率减小，减弱了异常信息对估值的影响。反之，参数估值增加有用量测信息的利用率；同理，当状态模型存在异常时，自适应因子α_k减小，参数估值时利用状态预测信息率减小，减弱模型异常的干扰，反之亦成立。若等价权且α=0时，和即为UKF算法得到的样本均值和方差。

(c)计算权值 $w_k^i=w_{k-1}^i\frac{p\left(y_k\right|{\stackrel{\&OverBar;}{x}}_k^{i^{*}})p\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}})}{q\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}},l_k)},$ 并归一化为 ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

(d)计算估计式 ${\hat{N}}_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\tilde{w}}_k^i\right)}^2.$

将所得结果与既定阙值进行比较，判断粒子退化的严重程度，越小，表明退化越严重。在这种情况下，可对上面获得的后验密度进行重采样，重新得到M个新的粒子，并赋予各个粒子相同权值1／M。

(e)计算非线性状态量估计值。重复上面步骤(b)。

上述步骤在选取重要性密度函数时，利用了两个重要的调节因子，即等价权因子和自适应因子。通过两者对UT变换后所得粒子采样点更合理的分配有用信息，为重要性采样过程提供更好的采样分布函数。

②渐消自适应Unscented粒子滤波：改进的渐消自适应Unscented粒子滤波是以粒子滤波为基本框架，融合渐消自适应滤波原理和UT变换过程，吸收各单一算法优点，建立参数可自适应调节的重要性密度分布函数，充分高效的利用最新量测信息，使其更接近真实的分布函数，从而使滤波算法具有更好的自适应性和鲁棒性。

Unscented粒子滤波算法主要利用UT变换得到采样点，实现对状态向量后验分布的近似。与蒙特卡罗方法不同，Unscented粒子滤波不是随机的从给定分布中进行采样，是取少数确定的Sigma点作为采样点。Sigma采样点为

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(20\right)$

式中λ=α²(N+v)代表尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度。对于不同的***模型和噪声假设，UT变换算法具有不同的形式，决定UT变换算法表达式的关键是确定Sigma点采样策略，即Sigma点个数、位置和权 值。

渐消自适应Unscented粒子滤波算法采用渐消因子限制滤波器的记忆长度，充分利用当前观测值不断修正预测值，并对未知的或不确切的***模型参数、噪声统计参数等进行估计和修正。算法主要步骤为：

(a)初始化，k=0时，其中k表示时刻。统一设置权值为其中k表示时刻，N表示粒子个数。

(b)计算Sigma点，设新样本为计算2N+1个Sigma采样点，利用UKF算法对粒子进行预测和更新，

其中各符号意义同上，下式中β一般根据先验知识来取值(对于高斯分布最佳取值为2)，W_j表示第j个Sigma点的权值，满足∑W_j=1，j＝0，1，…2N.

时间更新

$W_0^m=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})},W_0^c=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;}),j=0---\left(21\right)$

$W_j^c=W_j^m=W_j=1/2(N+\mathrm{\&lambda;}),j=\mathrm{1,2},...,2N---\left(22\right)$

${\mathrm{\&chi;}}_{k|k-1}^i=f\left({\mathrm{\&chi;}}_{k-1}^i\right)---\left(23\right)$

${\stackrel{\&OverBar;}{x}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{\mathrm{\&chi;}}_{j,k|k-1}^i---\left(24\right)$

$Y_{k|k-1}^i=h\left({\mathrm{\&chi;}}_{k|k-1}^i\right)---\left(25\right)$

${\stackrel{\&OverBar;}{y}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{Y}_{j,k|k-1}^i---\left(26\right)$

$P_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-x_{k|k-1}^i\right]\&CenterDot;{[{\mathrm{\&chi;}}_{j,k|k-1}^i,-x_{k|k-1}^i]}^T---\left(27\right)$

量测更新

$P_{x_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[Y_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]}^T---\left(28\right)$

$P_{l_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c[{(Y}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]\&CenterDot;{[Y_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]}^T---\left(29\right)$

$K_k=P_{x_k{l}_k}{P}_{l_k{l}_k}^{-1}---\left(30\right)$

此时利用渐消自适应扩展卡尔曼滤波思想计算渐消因子，利用式α_k，并计算

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k/k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i)---\left(31\right)$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T---\left(32\right)$

得到作为粒子采样的重要性密度函数，进行重要性采样。

(c)从重要性密度函中进行采样后，并对每个粒子计算权值

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(33\right)$

并计算归一化权值。

${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i---\left(34\right)$

(d)利用估计式判断粒子退化程度是否严重，然后从上面获得的后验密度进行重采样重新得到M个新的粒子，重新赋予各个粒子相同权值

(e)计算非线性状态量估计值。

${\hat{x}}_k=\underset{i=0}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i{x}_k^{i**}---\left(35\right)$

返回第(2)步，按新观测量递归计算下一时刻的状态估计值。

③模糊抗差自适应粒子滤波：

(I)基于模糊理论构造等价权：

为了得到既有较强抗差性又有较高自适应性的估值，可根据数据残差将权划分为：保权区(保持原观测值不变)、降权区(对观测值作抗差限制)以及拒绝区(权为零)。设计一维模糊控制器，构造权因子γ的步骤如下：

(a)模糊化。模糊化的作用是将输入的精确量转换成模糊化量，即将输入量(精确值)模糊化变为模糊变量(其中为标准化残差)，将确定的输入转化为由隶属度描述的模糊集。

具体过程：将输入变量的模糊子集划分为{太大，较大，正常}，简记为{B_e，B_c，B_n}，输入量化后，的论域为X＝{0，1，2，3，4}；输出γ的模糊子集为{极小， 较小，正常}，简记为{S_e，S_c，S_n}，其论域为Y大小分为5个等级，以表示不同的值，即Y＝{0，1，2，3，4}。分别对输入和输出γ进行模糊量化。

(b)根据人的直觉思维推理和实践经验，由输入量和输出量权因子γ的关系，设计模糊控制规则。如果太大，则γ极小；如果较大，则γ较小；如果正常，则γ正常。

根据模糊规则可以确定模糊关系为

R=(B_e×S_e)+(B_c×S_c)+(B_n×S_n)

其中，“×”表示模糊向量的笛卡尔乘积。经计算

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right]$

(c)根据模糊控制原理，由输入变量的模糊子集和模糊关系矩阵R通过模糊推理得到权因子为γ的一个模糊集合，得出最终的模糊控制量。

(d)将该模糊控制量进行解模糊得到精确的输出控制量，即权因子γ。模糊推理结果转化为精确值的过程称为反模糊化。在反模糊化处理过程时，采用最大隶属度原则。

(II)模糊抗差自适应粒子滤波算法

模糊抗差自适应粒子滤波(Fuzzy Robust Adaptively Particle Filter，FRAPF)算法的步骤如下：(a)初始化。在k＝0时刻，根据重点密度抽样出N个粒子，假定抽样出的每个粒子用表示，令k=1；(b)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。

误差方程为：

$V_k=A_k{\hat{X}}_k-Z_k---\left(36\right)$

式中，A_k为系数矩阵；Z_k为观测向量，其权矩阵为P_k；V_k为残差向量；为状态参数向量。取风险函数为

${V_k}^T{\stackrel{\&OverBar;}{P}}_k{V}_k+{({\stackrel{\&OverBar;}{X}}_k-{\hat{X}}_k)}^T{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}^{-1}({\stackrel{\&OverBar;}{X}}_k-{\hat{X}}_k)=\min ---\left(37\right)$

其中，为Z_k的等价权矩阵，即γ为权因子。求极值后，

设等价权矩阵为 $\stackrel{\&OverBar;}{P}=\mathrm{diang}({\stackrel{\&OverBar;}{P}}_1,{\stackrel{\&OverBar;}{P}}_{2,}...,{\stackrel{\&OverBar;}{P}}_k),$

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(38\right)$

预测残差含有未经观测信息修正的状态，更能反映动态***的扰动。k时刻第i个预测残差向量表示为

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(39\right)$

以预测残差为变量构造状态模型误差的判别统计量为

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(40\right)$

其中，为k时刻第i个状态预测信息，通过求出；表示预测残差的协方差矩阵；tr(·)为矩阵求迹算子。基于预测残差判别统计量的自适应因子函数为

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(41\right)$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0<c<2.5。

(c)更新：

$x_k^i=f(x_{k-1}^i,v_{k-1})---\left(42\right)$

由根据式(42)更新k时刻的粒子

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)})---\left(43\right)$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})}---\left(44\right)$

更新权值和归一化权值，i＝1，2，…，N。

(d)重采样：对所有粒子的权值按照降序进行排序，设置门限样本点数为N_th(通 常可选为N／2或N／3)，有效样本点数为N_eff，当N_eff<N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)滤波： $p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i)---\left(45\right)$

计算k时刻的滤波密度并进行再采样，令k=k+1，返回(b)。

抗差自适应滤波的自适应因子α_k既不单独处理模型误差协方差矩阵也不单独处理前一历元状态参数向量估计的协方差矩阵而是作用于整体的状态参数预报向量的等价协方差矩阵由于抗差自适应滤波对观测信息采用了抗差估计原理，当观测存在异常时，将动力模型信息作为一个整体，采用统一的自适应因子调整动力学模型信息对状态参数的整体贡献，从而得到可靠的滤波结果。

④自适应SVD-UKF滤波算法：

(I)奇异值分解：奇异值分解(SVD)是数值代数计算中稳定性和精度较好的一种矩阵分解方法，易于在计算机上实现。定义如下。

假定A∈R^m×n(m≥n)，则矩阵A的奇异值分解可表示为

$A={\mathrm{U\&Lambda;V}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T---\left(46\right)$

式(46)中，U∈R^m×m，Λ∈R^m×n，V∈R^n×n，S=diag(s₁，s₂，...，s_r)。s₁≥s₂≥…≥s_r≥0称为矩阵A的奇异值，U和V的列向量分别称为矩阵A的左、右奇异向量。

如果A^TA正定，则式(46)可简化为

$A=U\left[\begin{array}{c}S\\ 0\end{array}\right]V^T---\left(47\right)$

若A对称正定，则A＝USU^T，此时左奇异向量与右奇异向量相等，从而可以减少计算量。

(II)统计量及自适应因子的确定：以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。预测残差(新息)含有未经观测信息修正的状态，更能反映动态***受到的扰动。预测残差表示为

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{x}}_k\right)-y_k---\left(48\right)$

式(48)中，为k时刻状态预测信息。用预测残差构造状态模型误差判别统计量如下。

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

式(49)中，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子。

自适应因子函数选为

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.---\left(50\right)$

式(50)中，α_k表示自适应因子，c为经验常数，通常情况下1<c<2.5。

(III)自适应SVD-UKF算法步骤：自适应SVD-UKF算法步骤如下：

(a)初始化：对状态方程中的参数初始化，并计算Sigma点均值和协方差的权重系数w^(m)、w^(c)。

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

$w_0^{\left(m\right)}=\frac{1}{l+\mathrm{\&lambda;}},w_0^{\left(c\right)}=\frac{1}{l+\mathrm{\&lambda;}}+(1+{\mathrm{\&alpha;}}^2+\mathrm{\&beta;}),$

$w_i^{\left(m\right)}=w_i^{\left(c\right)}=\frac{1}{2(l+\mathrm{\&lambda;})},i=\mathrm{1,2},...,2l$

其中，α表示Sigma点相对均值的扩散程度(通常1e-4≤α≤1)，β是关于***先验信息的参数。对高斯分布，β=2最佳，P₀是初始状态协方差阵，l为***状态维数。

(b)奇异值分解、计算Sigma点向量：

计算特征点协方差矩阵和2l+1个Sigma点向量χ_i，k，下文中k∈1，2，...，∞。

$P_{k-1}=U_{k-1}{S}_{k-1}{V}_{k-1}^T---\left(51\right)$

${\mathrm{\&chi;}}_{i,k-1}=\left[\begin{array}{ccc}{\hat{X}}_{k-1}& {\hat{X}}_{k-1}+\mathrm{\&rho;}{U}_{i,k}\sqrt{S_{i,k}}& {\hat{X}}_{k-1}\end{array},-\mathrm{\&rho;}{U}_{i,k}\sqrt{S_{i,k}}\right]---\left(52\right)$

式(52)中，ρ为缩放比例因数，较为恰当的取值为S_i，k为奇异值分解因子。

(c)时间更新：

χ_i，k|k-1＝f(χ_i,k-1)+W_k i＝0，1，...，2l (53)

${\hat{X}}_{k|k-1}={\mathrm{\&chi;}}_{0,k|k-1}+\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(m\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\mathrm{\&chi;}}_{0,k|k-1})---\left(54\right)$

$S_{i,k|k-1}=\mathrm{svd}\left\{\right[\sqrt{w_i^{\left(c\right)}}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})\left]\right\}---\left(55\right)$

${\stackrel{\&OverBar;}{S}}_{i,k|k-1}=S_{i,k|k-1}/\sqrt{{\mathrm{\&alpha;}}_k}--\left(56\right)$

式(55)中，svd{·}为SVD分解算子，式(56)中的自适应因子α_k由式(50)确定，用α_k修正S_i，k|k-1。

${\mathrm{\&chi;}}_{i,k|k-1}=\left[\begin{array}{ccc}{\mathrm{\&chi;}}_{i,k-1}& {\mathrm{\&chi;}}_{i,k-1}+\mathrm{\&rho;}{U}_{i,k}{\stackrel{\&OverBar;}{S}}_{i,k|k-1}& {\mathrm{\&chi;}}_{i,k-1}-\mathrm{\&rho;}{U}_{i,k}{\stackrel{\&OverBar;}{S}}_{i,k|k-1}\end{array}\right]---\left(57\right)$

$P_{k|k-1}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})}^T+Q---\left(58\right)$

y_i,k|k-1=h(x_i，k|k-1)+E_k i=0，1，...，2l (59)

${\hat{y}}_{k|k-1}=y_{0,k|k-1}+\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(m\right)}(y_{i,k|k-1}-{\hat{y}}_{k|k-1})---\left(60\right)$

(d)量测更新

$S_{{\hat{y}}_k}=\mathrm{svd}\left\{\right[\sqrt{w_i^{\left(c\right)}}(y_{i,k|k-1}-{\hat{y}}_{k|k-1})\left]\right\}---\left(61\right)$

$P_{{\hat{y}}_k{\hat{y}}_k}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+R---\left(62\right)$

$P_{{\hat{X}}_k{\hat{y}}_k}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T---\left(63\right)$

$K_k=P_{{\hat{X}}_k{\hat{y}}_k}{P}_{{\hat{y}}_k{\hat{y}}_k}^{-1}/\left(S_{{\hat{y}}_k}^T{S}_{{\hat{y}}_k}\right)---\left(64\right)$

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})---\left(65\right)$

$P_k=P_{k|k-1}-K_k{P}_{{\hat{y}}_k{\hat{y}}_k}{K}_k^T---\left(66\right)$

式(58)中的Q与式(62)中的R分别为***噪声方差和量测噪声方差。

⑤自适应平方根Unscented粒子滤波：设***动力学方程为

X_k=Φ_k，k-1X_k-1+W_k (67)

式中，X_k是k时刻n维状态向量，Φ_k，k-1为n×n状态转移矩阵，W_k为***噪声向量，其协方差阵为

观测方程为

Y_k=H_kX_k+e_k (68)

式中，Y_k是k时刻m维观测向量，H_k为m×n维设计矩阵，e_k为观测噪声向量，其协方差阵为∑_k。W_k，W_j，e_k，e_j(j≠k)互不相关。

已知状态的初始概率密度函数为p(X₀|Y₀)=p(X₀)，则根据贝叶斯估计理论，非线性时变***的状态预测方程和状态更新方程分别为

p(X_k|Y_1：K-1)＝∫p(X_k|X_k-1)p(X_k-1|Y_1：k-1)dX_k-1 (69)

$p\left(X_k\right|Y_{1:k})=\frac{p\left(Y_k\right|X_k)p\left(X_k\right|Y_{1:k-1})}{p\left(Y_k\right|Y_{1:k-1})}---\left(70\right)$

式中，p(X_k|X_k-1)为状态转移密度，p(X_k-1|Y_1：k-1)为k-1时刻的后验密度；

p(X_k|Y_1：k-1)为先验分布，p(Y_k|X_k)为似然密度，p(Y_k|Y_1：k-1)为归一化常数，可由下式求得

p(Y_k|Y_1：k-1)=∫p(Y_k|X_k)p(X_k|Y_1：k-1)dX_k (71)

式(69)～(71)构成了递推贝叶斯估计。式(71)仅对某些动态***能获得解析解。基于随机采样的Monte Carlo方法可将积分运算转化为有限样本点的求和运算，即可得后验概率密度函数的近似表达形式。实际中的后验密度p(X_k|Y_1：k)可能是多变量、非标准概率分布，需要借助重要性采样算法对其采样，因而需要构造重要性函数。选择恰当的重要性函数能有效解决粒子滤波的粒子退化问题。

本项目采用UKF算法产生粒子滤波的重要性密度函数，该算法充分利用最新的观测数据，实时修正动力学模型和噪声统计参数引起的误差。自适应平方根UPF的步骤如下。

(a)初始化(k=0)：随机抽取N个初始粒子(i＝1，2，…，N)。假设 ${\hat{X}}_0^i=E\left[X_0^i\right],S_0^i=\mathrm{chol}\left\{E\right[(X_0^i-{\hat{X}}_0^i){(X_0^i-{\hat{X}}_0^i)}^T\left]\right\},w_0^i=p\left(Y_0\right|X_0^i).$ 其中，和分别表示初始时刻第i个粒子及其估计值，表示初始时刻第i个Cholesky分解因子，表示第i个粒子的初始化权值，chol{·}表示矩阵的Cholesky分解算子。

(b)采用自适应平方根UKF滤波算法更新每个粒子得到 表示k时刻第i个粒子的协方差。

(b1)计算Sigma点和权值

$\left\{\begin{array}{cc}{X}_{0,k-1}^i={\hat{x}}_{k-1}^i& \\ X_{j,k-1}^i={\hat{X}}_{k-1}^i+\sqrt{(n+\mathrm{\&lambda;})}{S}_{k-1}^i& j=1,...,n\\ X_{j,k-1}^i={\hat{X}}_{k-1}^i-\sqrt{(n+\mathrm{\&lambda;})}{S}_{k-1}^i& j=n+1,...,2n\end{array}---\right.\left(72\right)$

$\left\{\begin{array}{cc}{W}_0^m=\mathrm{\&lambda;}/(n+\mathrm{\&lambda;})& \\ W_0^c=\mathrm{\&lambda;}/(n+\mathrm{\&lambda;})+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})& \\ W_j^m=W_j^c=0.5/(n+\mathrm{\&lambda;})& j=1,...,2n\end{array}\right.---\left(73\right)$

其中，λ为尺度参数，n表示***状态维数，α表示Sigma点围绕均值的扩散程度(通常1e-4≤α≤1)，β是关于***先验信息的参数，对高斯分布β=2最佳，表示第j个Sigma点，W_j为其权值。

(b2)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子。

预测残差(或新息)含有未经观测信息修正的状态，更能反映动态***的扰动。k时刻第i个预测残差向量表示为

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(74\right)$

以预测残差为变量构造状态模型误差的判别统计量为

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(75\right)$

其中，为k时刻第i个状态预测信息，通过求出，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子。

基于预测残差判别统计量的自适应因子函数为

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(76\right)$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0<c<2.5。

(b3)时间更新(状态预测)

$X_{k\backslash k-1}^i={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}^i+W_k---\left(77\right)$

${\hat{X}}_{k/k-1}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^m{X}_{j,k/k-1}^i---\left(78\right)$

$S_{k/k-1}^i=\mathrm{qr}\left\{\right[\sqrt{W_1^c}(X_{1:2n,k/k-1}^i-{\hat{X}}_{k/k-1}^i)\sqrt{{\mathrm{\&Sigma;}}_{W_k}}\left]\right\}---\left(79\right)$

$S_{k/k-1}^i=\mathrm{cholupdate}\{S_{k/k-1}^i,X_{0,k/k-1}^i-{\hat{X}}_{k/k-1}^i,W_0^c\}---\left(80\right)$

${\stackrel{\&OverBar;}{S}}_{k/k-1}^i=S_{k/k-1}^i/\sqrt{{\mathrm{\&alpha;}}_k^i}---\left(81\right)$

$X_{k/k-1}^i=\left[\begin{array}{ccc}{\hat{X}}_{k/k-1}^i& {\hat{X}}_{k/k-1}^i+\sqrt{n+\mathrm{\&lambda;}}{\stackrel{\&OverBar;}{S}}_{k/k-1}^i& {\hat{X}}_{k/k-1}^i-\sqrt{n+\mathrm{\&lambda;}}{\stackrel{\&OverBar;}{S}}_{k/k-1}^i\end{array}\right]---\left(82\right)$

$Y_{k/k-1}^i=H_k{X}_{k/k-1}^i+e_{k-1}---\left(83\right)$

${\hat{Y}}_{k/k-1}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^c{Y}_{j,k/k-1}^i---\left(84\right)$

其中，和分别表示k时刻至k-1时刻第i个粒子的状态转移及其估计，cholupdate{·}表示Cholesky分解因子更新算子，式(81)利用自适应因子修正Cholesky分解因子

(b4)测量更新(状态估计)

$S_{{\hat{y}}_k}^i=\mathrm{qr}\left\{\right[\sqrt{W_1^c}(Y_{1:2n,k/k-1}^i-{\hat{Y}}_{k/k-1}^i)\mathrm{}\sqrt{{\mathrm{\&Sigma;}}_{k-1}^i}\left]\right\}---\left(85\right)$

$S_{{\hat{y}}_k}^i=\mathrm{cholupdate}\{S_{{\hat{y}}_k}^i,Y_{0,k/k-1}^i-{\hat{Y}}_{k/k-1}^i,W_0^c\}---\left(86\right)$

${\mathrm{\&Sigma;}}_{{\hat{x}}_k{\hat{y}}_k}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^c[X_{j,k/k-1}^i-{\hat{X}}_{k/k-1}^i]{[Y_{j,k/k-1}^i-{\hat{Y}}_{k/k-1}^i]}^T---\left(87\right)$

$K_k^i=({\mathrm{\&Sigma;}}_{{\hat{x}}_k{\hat{y}}_k}^i/{\left(S_{{\hat{y}}_k}^i\right)}^T)/S_{{\hat{y}}_k}^i---\left(88\right)$

${\hat{X}}_k^i={\hat{X}}_{k/k-1}^i+K_k^i(Y_k^i-{\hat{Y}}_{k/k-1}^i)---\left(89\right)$

$U^i=K_k^i{S}_{{\hat{y}}_k}^i---\left(90\right)$

$S_k^i=\mathrm{cholupdate}\{{\stackrel{\&OverBar;}{S}}_{k/k-1}^i,U^i,-1\}---\left(91\right)$

${\mathrm{\&Sigma;}}_k^i=S_k^i{\left(S_k^i\right)}^T---\left(92\right)$

该步骤中使用了线性代数中的QR分解、Cholesky分解，以Cholesky分解因子的形式直接传播和更新状态协方差矩阵，从而增强了状态协方差矩阵更新过程中的数值稳定性，保证了协方差矩阵的正定性。其中，qr{·}表示矩阵的QR分解。

(c)重要性采样权值计算：令重要性分布函数采样粒子N(·)表示正态分布。分别通过 $w_k^i=w_{k-1}^i\frac{p(Y_k/X_k^i)p(X_k^i/X_{k-1}^i)}{q\left({\stackrel{\&OverBar;}{X}}_k^i\right|{\stackrel{\&OverBar;}{X}}_{0:k-1}^i,Y_{1:k})},$ 更新权值和归一化权值，i＝1，…，N。

(d)重采样：

设置门限样本点数为N_th(通常可选为N／2或N／3)，有效样本点数为N_eff，当N_eff<N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)状态更新：

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{X}}_k^i{\tilde{w}}_k^i---\left(93\right)$

${\widehat{\mathrm{\&Sigma;}}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i({\hat{X}}_k-{\tilde{X}}_k^i){({\hat{X}}_k-{\tilde{X}}_k^i)}^T---\left(94\right)$

Claims (6)

1.一种自主组合导航***，其特征在于：将SINS作为主导航***，SAR、CNS作为辅助导航***，构成SINS/SAR/CNS组合导航***，其具体步骤如下：

首先，设计SINS/SAR和SINS/CNS导航子滤波器，通过计算获得组合导航***状态的两组局部最优估计值和局部最优误差协方差阵然后，采用联邦滤波技术，将两组局部最优估计值送入主滤波器中进行信息融合，得到***状态的全局最优估计值和全局最优误差协方差阵最后，利用获得的状态的全局最优估计值对捷联惯导***的误差进行实时校正；SINS/SAR/CNS组合导航***最优估计融合算法为

$\left\{\begin{array}{c}{\mathrm{\&Sigma;}}_{\hat{X},g}={({\mathrm{\&Sigma;}}_{\hat{X},1}^{-1}+{\mathrm{\&Sigma;}}_{\hat{X},2}^{-1})}^{-1}\\ {\hat{X}}_g={\mathrm{\&Sigma;}}_{\hat{X},g}({\mathrm{\&Sigma;}}_{\hat{X},1}^{-1}{\hat{X}}_1+{\mathrm{\&Sigma;}}_{\hat{X},2}^{-1}{\hat{X}}_2)\end{array}\right.;$

所述SINS/SAR/CNS组合导航***，其数学模型包括状态方程、量测方程和自主导航高精度非线性滤波算法：

(1)状态方程：将SAR和CNS的导航误差考虑为高斯白噪声，且不列为组合导航***的状态量，只将SINS的***误差考虑为SINS/SAR/CNS组合导航***的***状态量；

选取东北天E-N-U地理坐标系g作为组合导航***的导航坐标系n，根据惯性导航***的误差方程，SINS/SAR/CNS组合导航***的状态x(t)选取为：

$x\left(t\right)={\&lsqb;{\left(\mathrm{\&delta;}q\right)}^T,{\left(\mathrm{\&delta;}V\right)}^T,{\left(\mathrm{\&delta;}P\right)}^T,{\mathrm{\&epsiv;}}^T,{\&dtri;}^T\&rsqb;}^T;$

式中，δq＝[δq₀、δq₁、δq₂、δq₃]^T为SINS的姿态误差四元数；δV＝[δV_E、δV_N、δV_U]^T为SINS东、北、天方向的速度误差；δP＝[δL、δλ、δh]^T为SINS的纬度、经度和高度误差；ε＝[ε_x、ε_y、ε_x]^T表示陀螺的随机漂移；是加速度计的常值偏置；

根据上式可得，SINS/SAR/CNS组合导航***的状态方程为：

$\stackrel{\&CenterDot;}{x}=f(x,t)+G\left(t\right)w\left(t\right);$

式中，f(x，t)为***的状态阵；w(t)＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az]^T表示***噪声，[w_gx、w_gy、w_gz]为陀螺的白噪声，[w_ax、w_ay、w_az]为加速度计的白噪声；G(t)为***的噪声驱动矩阵，且***状态阵和噪声驱动阵分别为：

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\&omega;}}_{in}^n-{\mathrm{BC}}_b^c{\mathrm{\&epsiv;}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;{\mathrm{\&delta;V}}^n-(2{\mathrm{\&delta;\&omega;}}_{ie}^n+{\mathrm{\&delta;\&omega;}}_{en}^n)\&times;V^n+C_b^c{\&dtri;}^b\\ M\mathrm{\&delta;}V+N\mathrm{\&delta;}L\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right];$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^c\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$

式中，B表示姿态四元数的数乘，I表示单位阵，表示导航坐标系n与计算坐标系c之间的转换矩阵，表示载体坐标系b与计算坐标系c之间的转换矩阵，ε^b为陀螺误差，表示计算坐标系c与导航坐标系n之间的转换矩阵，f^b为加速度计的量测，即比力，和分别是地球自转角速度和位置角速度，δVⁿ为速度误差，为加速度计误差，M和N分别为速度误差和位置误差系数阵，δV为载体速度误差，δL为载体纬度误差；

(2)量测方程，包括以下量测方程：

①SINS/SAR子***量测方程：可以将捷联惯导输出的航向角信息和位置信息与合成孔径雷达输出的航向角信息、水平位置信息以及气压高度表输出的载体高度信息的差作为量测量，SINS/SAR组合导航***的量测方程为：

z₁(t)＝h₁(x，t)+v₁(t)；

式中，h₁(x，t)为量测方程的非线性函数，v(t)＝[δψ_S，δL_S，δλ_S，δh_e]^T为量测白噪声，均值为零；

②SINS/CNS子***量测方程：SINS/CNS组合导航子***的量测方程为：

z₂(t)＝h₂(t)x(t)+v₂(t)；

式中，h₂(t)为量测矩阵，v₂(t)＝[δq_C0，δq_C1，δq_C2，δq_C3，δL_C，δλ_C，δh_e]^T为量测白噪声，均值为零；

(3)自主导航高精度非线性滤波算法：设计了一套适合于SINS/CNS/SAR自主组合导航***的高精度、非线性滤波算法，该套算法包括：①抗差自适应Unscented粒子滤波算法；②渐消自适应Unscented粒子滤波；③模糊抗差自适应粒子滤波；④自适应SVD-UKF滤波算法；⑤自适应平方根Unscented粒子滤波。

2.根据权利要求1所述的自主组合导航***，其特征在于：所述抗差自适应Unscented粒子滤波算法步骤如下：

(a)初始化：根据初始均值和均方差抽取N个粒子，在k＝0时刻， 设置权值为

(b)在k＝1，2，…，N时刻，按照下面次序计算：

(b1)计算等价权和自适应因子α，选用IGG方案构造等价权函数，IGG法属于降权函数，即对量测值做抗差限制，若取其倒数，则定义为方差膨胀因子函数：

设等价权矩阵为

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.;$

根据需要也可以采用另一种表达式：

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(h_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.;$

其中k₀∈(1，1.5)，k₁∈(3，8)，V_k为观测值l_k的残差向量，A_k为系数矩阵，为当前时刻状态参数估计值；自适应因子选取如下：

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.;$

其中c₀∈(1，1.5)，c₁∈(3，8)，tr(·)表示矩阵的迹，为预报值，即前者通过对残差的判断来选取，而后者根据状态估计值与预报值之差来选取；

(b2)计算Sigma点，由UKF算法更新粒子得到且满足q(·)为重要性概率密度函数，N(·)为正态分布函数，也称后验密度，为***状态量的估计值，为k-1时刻第i个***状态量的估计值，为***状态量的预测值，l_k为观测值，为估计均方误差阵，设新样本为2N+1个Sigma点采样点为：

${\mathrm{\&chi;}}_{k-1}^i=\&lsqb;{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}\&rsqb;---\left(15\right)$

式中λ＝α²(N+v)表示尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度，W_j表示第j个Sigma点的权值，满足∑W_j＝1，j＝0，1，…2N；

(c)计算权值并归一化为

式中，p(·)为概率密度分布函数，y_k为***量测量，为采样粒子估计值，为k-1时刻第i个采样粒子估计值；

(d)计算估计式

将所得结果与既定阙值进行比较，判断粒子退化的严重程度，越小，表明退化越严重；在这种情况下，可对上面获得的后验密度进行重采样，重新得到M个新的粒子，并赋予各个粒子相同权值1/M；

(e)计算非线性状态量估计值：重复上面步骤(b)；

上述步骤在选取重要性密度函数时，利用了两个重要的调节因子，即等价权因子和自适应因子；通过两者对UT变换后所得粒子采样点更合理的分配有用信息，为重要性采样过程提供更好的采样分布函数。

3.根据权利要求1所述的自主组合导航***，其特征在于：所述渐消自适应Unscented粒子滤波算法主要利用UT变换得到采样点，实现对状态向量后验分布的近似；与蒙特卡罗方法不同，Unscented粒子滤波不是随机的从给定分布中进行采样，是取少数确定的Sigma点作为采样点；Sigma采样点为

${\mathrm{\&chi;}}_{k-1}^i=\&lsqb;{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}\&rsqb;;$

式中λ＝α²(N+v)代表尺度因子，v为二阶尺度因子，N为采样粒子个数，α决定采样点对预测均值的分散程度，为k-1时刻第i个***状态量的预测值，为k-1时刻第i个估计均方误差阵；对于不同的***模型和噪声假设，UT变换算法具有不同的形式，决定UT变换算法表达式的关键是确定Sigma点采样策略，即Sigma点个数、位置和权值；其算法主要步骤为：

(a)初始化，k＝0时，其中k表示时刻；统一设置权值为其中k表示时刻，N表示粒子个数；

(b)计算Sigma点，设新样本为计算2N+1个Sigma采样点，利用UKF算法对粒子进行预测和更新；利用渐消自适应扩展卡尔曼滤波思想计算渐消因子，利用式α_k，并计算

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i);$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T;$

得到作为粒子采样的重要性密度函数，进行重要性采样，

式中，K_k为增益矩阵，y_k为***量测量，为k-1时刻第i个***量测量预测值；

(c)从重要性密度函中进行采样后，并对每个粒子计算权值

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)};$

并计算归一化权值；

${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\&Sigma;}}{w}_k^i;$

(d)利用估计式判断粒子退化程度是否严重，然后从上面获得的后验密度进行重采样重新得到M个新的粒子，重新赋予各个粒子相同权值

(e)计算非线性状态量估计值：

${\hat{x}}_k=\underset{i=0}{\overset{N}{\&Sigma;}}{\tilde{w}}_k^i{x}_k^{i**},$

式中，为***状态量的重采样估计值；

返回步骤(b)，按新观测量递归计算下一时刻的状态估计值。

4.根据权利要求1所述的自主组合导航***，其特征在于：所述模糊抗差自适应粒子滤波包括基于模糊理论构造等价权和具体算法步骤，其中：

(I)基于模糊理论构造等价权：根据数据残差将权划分为：保权区，保持原观测值不变，、降权区，对观测值作抗差限制以及拒绝区，权为零；设计一维模糊控制器，构造权因子γ的步骤如下：

(a)模糊化：将输入量精确值，模糊化变为模糊变量，其中为标准化残差，将确定的输入转化为由隶属度描述的模糊集；其具体过程为：将输入变量的模糊子集划分为{太大，较大，正常}，简记为{B_e，B_c，B_n}，输入量化后，的论域为X＝{0，1，2，3，4}；输出γ的模糊子集为{极小，较小，正常}，简记为{S_e，S_c，S_n}，其论域为Y大小分为5个等级，以表示不同的值，即Y＝{0，1，2，3，4}；分别对输入和输出γ进行模糊量化；

(b)根据人的直觉思维推理和实践经验，由输入量和输出量权因子γ的关系，设计模糊控制规则；如果太大，则γ极小；如果较大，则γ较小；如果正常，则γ正常；根据模糊规则可以确定模糊关系为

R＝(B_e×S_e)+(B_c×S_c)+(B_n×S_n)；

其中，“×”表示模糊向量的笛卡尔乘积；经计算

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right];$

(c)根据模糊控制原理，由输入变量的模糊子集和模糊关系矩阵R通过模糊推理得到权因子为γ的一个模糊集合，得出最终的模糊控制量；

(d)将该模糊控制量进行解模糊得到精确的输出控制量，即权因子γ；模糊推理结果转化为精确值的过程称为反模糊化；在反模糊化处理过程时，采用最大隶属度原则；

(II)模糊抗差自适应粒子滤波算法的步骤如下：

(a)初始化：在k＝0时刻，根据重点密度抽样出N个粒子，假定抽样出的每个粒子用表示，令k＝1；

(b)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子：以预测残差为变量构造状态模型误差的判别统计量为：

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{tr\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}};$

其中，通过求出，f_k(g)为***状态转移矩阵；表示预测残差的协方差矩阵；tr(·)为矩阵求迹算子；基于预测残差判别统计量的自适应因子函数为：

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.;$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0＜c＜2.5；

(c)更新：

$x_k^i=f(x_{k-1}^i,v_{k-1});$

由限据式(42)更新k时刻的粒子

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\&Sigma;}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)});$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})};$

更新权值和归一化权值，i＝1，2，…，N；

(d)重采样：对所有粒子的权值按照降序进行排序，设置门限样本点数为N_th，通常可选为N/2或N/3，有效样本点数为N_eff，当N_eff＜N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)滤波：

$p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i);$

计算k时刻的滤波密度并进行再采样，令k＝k+1，返回(b)。

5.根据权利要求1所述的自主组合导航***，其特征在于：所述自适应SVD-UKF滤波算法包括：

(I)奇异值分解：奇异值分解定义如下：

假定A∈R^m×n(m≥n)，则矩阵A的奇异值分解可表示为

$A={\mathrm{U\&Lambda;V}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T;$

式中，U∈R^m×m，Λ∈R^m×n，V∈R^n×n，S＝diag(s₁，s₂，...，s_r)；s₁≥s₂≥...≥s_r≥0称为矩阵A的奇异值，U和V的列向量分别称为矩阵A的左、右奇异向量；

(II)统计量及自适应因子的确定：以预测残差为变量，构造状态模型的误差判别统计量及自适应因子，预测残差表示为：

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{X}}_k\right)-y_k;$

式中，为k时刻状态预测信息；用预测残差构造状态模型误差判别统计量如下：

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/tr({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\left)\right)}^{1/2};$

式中，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子；

自适应因子函数选为：

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.;$

式中，α_k表示自适应因子，c为经验常数，通常情况下1＜c＜2.5；

(III)自适应SVD-UKF算法步骤如下：

(a)初始化：对状态方程中的参数初始化，并计算Sigma点均值和协方差的权重系数w^(m)、w^(c)；(b)奇异值分解、计算Sigma点向量；(c)时间更新；(d)量测更新。

6.根据权利要求1所述的自主组合导航***，其特征在于：所述自适应平方根Unscented粒子滤波步骤如下：

(a)初始化，k＝0：随机抽取N个初始粒子假设其中，和分别表示初始时刻第i个粒子及其估计值，表示初始时刻第i个Cholesky分解因子，表示第i个粒子的初始化权值，chol{·}表示矩阵的Cholesky分解算子；

(b)采用自适应平方根UKF滤波算法更新每个粒子得到 表示k时刻第i个粒子的协方差，步骤如下：

(b1)计算Sigma点和权值：

(b2)以预测残差为变量，构造状态模型的误差判别统计量及自适应因子；

预测残差或新息含有未经观测信息修正的状态，更能反映动态***的扰动，k时刻第i个预测残差向量表示为：

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k;$

以预测残差为变量构造状态模型误差的判别统计量为：

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{tr\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}};$

其中，H_k表示量测矩阵，Y_k表示***量测量，为k时刻第i个状态预测信息，通过求出，表示预测残差的协方差矩阵，tr(·)为矩阵求迹算子；

基于预测残差判别统计量的自适应因子函数为：

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.;$

式中，表示k时刻第i个自适应因子，c为经验常数，一般取1.0＜c＜2.5；

(b3)时间更新，状态预测；

(b4)测量更新，状态估计：该步骤中使用了线性代数中的QR分解、Cholesky分解，以Cholesky分解因子的形式直接传播和更新状态协方差矩阵，从而增强了状态协方差矩阵更新过程中的数值稳定性，保证了协方差矩阵的正定性；

(c)重要性采样权值计算：令重要性分布函数采样粒子N(·)表示正态分布；分别通过 更新权值和归一化权值，i＝1，…，N，

式中，为***状态量的估计值，为k时刻第i个状态预测信息；

(d)重采样：设置门限样本点数为N_th，通常可选为N/2或N/3，有效样本点数为N_eff，当N_eff＜N_th时，对粒子集重新采样，得到新的粒子集并重新设定权值为

(e)状态更新：

式中，为当前时刻***状态量的估计值。