CN104020480B - 一种带自适应因子的交互式多模型ukf的卫星导航方法 - Google Patents

Abstract

本发明涉及一种带自适应因子的交互式多模型UKF的卫星导航方法，属于导航控制领域。本发明方法首先利用残差序列的自协方差矩阵作为校正量，设计平滑滤波器将此校正量与陈旧自适应因子融合，得到一个新的自适应因子，并利用此自适应因子实时调整过程噪声协方差矩阵，有效地降低了***中噪声统计特性未知造成的定位误差。然后，采用交互式多模型算法(IMM)设置模型集M，并根据测量残差实时调整模型概率实现模型间的软切换，降低了***模型不精确造成的定位误差。本发明方法有效地改善了卫星导航***中复杂机动载体在模型和噪声统计特性未知的条件下的定位精度。

Description

一种带自适应因子的交互式多模型UKF的卫星导航方法

技术领域

本发明涉及一种带自适应因子的交互式多模型UKF的卫星导航方法，属于导航控制领域。

背景技术

接收机定位算法是影响卫星导航***定位精度的关键。长期以来最小二乘法以其原理简单，易于实现等优点得到了广泛的应用。但是最小二乘法没有把不同时刻的估计值联系起来，定位结果显得分散、杂乱无章，这对导航算法的优化制造了困难。为克服最小二乘法在估计性能方面的不足，学者们将非线性滤波算法应用于导航领域。对于非线性***的滤波问题，由于需要处理无穷维的积分运算，在理论上很难找到严格的最优解，一般都是通过近似的方法来求解。当前非线性滤波方法主要分为三类，第一类是函数近似的方法，主要采用泰勒级数展开或插值多项式展开的方法对非线性函数进行近似，其代表为扩展卡尔曼滤波和插值滤波；第二类是确定性采样近似，它通过挑选一组sigma点作为样本，并通过非线性函数传播获得***的验后均值和方差。其典型代表是无迹卡尔曼滤波；第三类是随机采样近似，主要指基于蒙特卡洛方法的仿真，其代表是粒子滤波。在理论上粒子滤波是最优的，且适用于非高斯噪声的情况。但是粒子滤波存在粒子退化、计算量大、实时性差等问题。

扩展卡尔曼滤波(ExtendKalmanFilter，EKF)是函数近似方法的典型代表，也是应用最为广泛的滤波算法之一，在很多领域得到了广泛的应用。但是EKF采用一阶泰勒级数展开式近似线性化的方法会产生模型误差，导致滤波精度下降。为克服EKF在滤波精度方面的不足，学者们提出了无迹卡尔曼滤波(UnsignedKalmanFilter，UKF)，成为第二类方法-确定性采样近似的典型方法之一。UKF挑选一组sigma点作为样本，并通过非线性函数传播获得***的验后均值和方差，避免了非线性***的线性化过程，其估计精度可以达到二阶泰勒级数展开的精度，而与之计算量相当的EKF只能达到一阶展开的精度。

但是EKF和UKF存在共同的不足即需要准确的***模型和噪声统计特性，对于很多***，这些条件都难以满足，基于此，学者们提出了自适应滤波算法。目前自适应滤波算法主要有噪声方差匹配法和模型匹配法。⑴噪声方差匹配法一般采用实时调整过程噪声协方差矩阵的方法实现，其中以Sage-Husa自适应算法为代表。Sage-Huse自适应滤波算法以其原理简单得到了一定的利用，但它对滤波初值比较敏感，且极易引起滤波发散，因此在应用时要进行滤波发散的抑制，导致算法繁琐，计算量大；A.H.Mohamed提出基于极大似然估计准则的自适应算法，该算法根据估计残差推导了Q阵和R阵的更新公式。HalilErsinSoken为矩阵Q增加自适应因子来实时调整矩阵Q的值，当估计精确时Q阵保持不变，当估计发生偏差时，由测量残差重新确定噪声矩阵的值。这两种方法实时性好，但都需要计算雅克比矩阵，增加了UKF的计算量。而且，这两种方法都完全依靠测量残差确定噪声协方差矩阵，估计稳定性弱；为增强以上算法的稳定性，学者们提出了主从式AUKF，它在从UKF中根据测量残差更新噪声协方差矩阵，并利用该方差矩阵去校正主UKF，这种算法稳定性较好但是计算量大；模糊自适应滤波算法以测量残差作为输入来制定模糊规则，实时调整Q阵的值，算法简单但灵活度差；汪秋婷在其博士论文中通过给过程噪声矩阵增加自适应因子来增加模型的精确性，原理简单，计算量小，但对测量残差的统计不够精确。⑵模型匹配法主要采用IMM算法，该算法设置模型集M，并根据估计残差实时调整模型概率，实现模型间的软切换。该方法解决了单一模型对载体运动状态描述不全面而导致的滤波精度低的问题。IMM算法最开始应用于目标跟踪，现在在导航领域也得到了越来越多的应用。实验证明，当***模型不确定时，该方法能明显提高导航***的定位精度，但该算法无法校正噪声统计特性未知造成的误差。

发明内容

本发明针对卫星导航***中复杂机动载体运动模型和噪声统计特性未知造成滤波精度低的问题提出了一种带自适应因子的交互式多模型UKF的卫星导航方法，有效地改善了卫星导航***中复杂机动载体在模型和噪声统计特性未知的条件下的定位精度。

本发明的目的是通过以下技术方案实现的。

一种带自适应因子的交互式多模型UKF的卫星导航方法，具体包括如下步骤：

步骤1：建立***模型

步骤1.1：建立基于全球定位***(GPS)的地面载体的状态方程。

为全面描述载体的运动状态和钟差漂移频率，用符号X表示全球定位***(GPS)状态方程的状态变量， $X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x,}y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z,}{\mathrm{\δt}}_u,\mathrm{\δ}{\stackrel{\·}{t}}_u]}^T.$ 其中，x，分别代表世界大地坐标***(WorldGeodeticSystem一1984CoordinateSystem，WGS-84坐标系)下X_T轴上的位置、速度和加速度；y，分别代表WGS-84坐标系下Y_T轴上的位置、速度和加速度；z，分别代表WGS-84坐标系下Z_T轴上的位置、速度和加速度；δt_u和分别代表载体接收机钟差和钟差漂移频率。建立模型集(用符号M表示)，模型集M包含描述载体运动状态的当前统计模型(用符号CS表示)、常加速度模型(用符号CA表示)和常速转弯模型(用符号CT表示)，即M＝[CS,CA,CT]。接收机钟差变化率用一阶马尔科夫模型描述，当前统计模型CS的状态方程如公式(1)所示；常加速度模型CA的状态方程如公式(2)所示；常速转弯模型CT的状态方程如公式(3)所示。

$X_k^1={\mathrm{\Φ}}_1{X}_{k-1}^1+{\mathrm{\Ψ}}_{k-1}^1{w}_{k-1}^1---\left(1\right)$

$X_k^2={\mathrm{\Φ}}_2{X}_{k-1}^2+{\mathrm{\Ψ}}_{k-1}^2{w}_{k-1}^2---\left(2\right)$

$X_k^3={\mathrm{\Φ}}_3{X}_{k-1}^3+{\mathrm{\Ψ}}_{k-1}^3{w}_{k-1}^3---\left(3\right)$

其中，分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态，其中i＝1,2,3，即：表示***在当前统计模型CS下k时刻的状态；表示***在常加速度模型CA下k时刻的状态；表示***在常速转弯模型CT下k时刻的状态；k为正整数；Φ_i为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵，Φ_i=diag(Φ_i，x，Φ_i，y，Φ_i，z，Φt，u)，Φ_i，x表示WGS-84坐标系下X_T轴上的状态转移矩阵；表示WGS-84坐标系下Y_T轴上的状态转移矩阵；Φ_i,z表示WGS-84坐标系下Z_T轴上的状态转移矩阵；Φ_t,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵；是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子；是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵。

用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k-1)时刻的过程噪声协方差矩阵， E[·]表示期望值运算，表示WGS-84坐标系下描述X_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Y_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Z_T轴上载体运动状态的过程噪声协方差矩阵；表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵；

步骤1.2：建立基于全球定位***(GPS)的地面载体的观测方程

采用伪距作为观测量建立观测方程。在k时刻，接收机观测到的GPS卫星数量用N表示，则建立此时刻CS模型，CA模型和CT模型的伪距观测方程如公式(4)所示。

${\mathrm{\ρ}}_n^k=\sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k+{\mathrm{\ϵ}}_n^k---\left(4\right)$

其中，表示在k时刻观测到的第n颗GPS卫星的伪距，n＝1,2…N，N表示k时刻观测到的GPS卫星的总数；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下X_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Y_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Z_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下X_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Y_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Z_T轴上的位置；表示k时刻地面载体接收机钟差；表示k时刻观测到的第n颗GPS卫星的观测误差。

为下文描述算法方便，建立CS模型，CA模型和CT模型的观测方程为公式(5)。

$y_k^i=h\left(X_k^i\right)+{\mathrm{\ϵ}}_k---\left(5\right)$

其中，，为***的观测量， $h\left(X_k^i\right)=\left[\begin{array}{c}\sqrt{{(x_k^i-x_1^k)}^2+{(y_k^i-y_1^k)}^2+{(z_k^i-z_1^k)}^2}+{\mathrm{\δt}}_u^k\\ \sqrt{{(x_k^i-x_2^k)}^2+{(y_k^i-y_2^k)}^2+{(z_k^i-z_2^k)}^2}+{\mathrm{\δt}}_u^k\\ \·\·\·\\ \sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k\end{array}\right],$ 观测方程中的非线性函数，为***的观测误差。

用R_k表示公式(5)的观测误差的协方差矩阵，

步骤2：设置CS模型，CA模型和CT模型0时刻的滤波初始值，同时设置当前定位任务的结束时间。

在步骤1操作的基础上，分别设置CS模型、CA模型和CT模型的状态的估计初值，用符号和表示；然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值，用符号和表示；并设置***运行的结束时间。

步骤3：得到CS模型，CA模型和CT模型在k时刻的滤波初始值。

使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k-1)时刻的状态的估计初值和，以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和，通过公式(6)和公式(7)进行交互混合，分别得到CS模型，CA模型和CT模型中k时刻的状态的估计初值，分别用 和表示，以及CS模型，CA模型和CT模型中k时刻的估计误差协方差矩阵初值，分别用和表示。

${\hat{X}}_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{\hat{X}}_{i=1}^i{u}_{k-1|k-1}^{i|j}---\left(6\right)$

其中，分别表示CS模型，CA模型和CT模型在k时刻滤波的状态初值；j＝1,2,3，分别代表CS模型、CA模型和CT模型；i＝1,2,3，分别表示CS模型，CA模型和CT模型在k-1时刻状态的最优估计值；表示k-1时刻模型i和模型j的混合概率，π_ij为模型i到模型j的模型转移概率，其值由人为设定；为模型i在k时刻的概率，的初值由人为设定，并在以后各个时刻由步骤6更新；代表模型j的归一化常数，

$P_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{u}_{k-1|k-1}^{i|j}[P_{k-1}^i+({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j}){({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j})}^T]---\left(7\right)$

其中，i＝1,2,3，分别表示CS模型、CA模型和CT模型在k-1时刻滤波误差的方差矩阵。分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值。

步骤4：对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波(AdaptiveUnsignedKalmanFilter,AUKF)，得到各模型在k时刻的滤波值。具体操作步骤为：

步骤4.1：用表示***状态的维数，即步骤1中得到的CS模型，CA模型和CT模型中状态方程的维数，通过公式(8)计算模型j在k时刻个初始西格马(sigma)点，其中第r个sigma点用用符号表示，且r为整数；

其中，表示平方根矩阵的第r列(或第r行)。

用公式(9)计算第一类权系数，该权系数用于求每个sigma点的一阶统计特性。模型j的第r个sigma点的第一类权系数用符号表示，即：表示CS模型的第r个sigma点第一类权系数，表示CA模型的第r个sigma点第一类权系数，表示CT模型的第r个sigma点第一类权系数；用公式(10)计算第二类权系数，该权系数用于求每个sigma点的二阶统计特性。模型j的第r个sigma点的第二类权系数用符号表示，即：表示CS模型的第r个sigma点第二类权系数，表示CA模型的第r个sigma点第二类权系数，表示CT模型的第r个sigma点第二类权系数；

其中，λ为一个复合刻度参数，其值通过公式(11)求解，参数α决定sigma点的散布程度，其值由人为设定，1e^-4≤α≤1；参数β用来描述全球定位***状态方程的状态变量X的分布信息，在高斯情况下，β的最优值为2。

$\mathrm{\λ}={\mathrm{\α}}^2(\tilde{n}+\mathrm{\κ})-\tilde{n}---\left(11\right)$

其中，参数κ取0。

步骤4.2：通过公式(12)至公式(14)进行时间更新。

${\mathrm{\ξ}}_k^{j,r}={\mathrm{\Φ}}_j{\mathrm{\ξ}}_{k-1}^{j,r}---\left(12\right)$

其中，表示模型j在k时刻经过一步转移后的第r个西格马(sigma)点；Φ_j为模型j的状态转移矩阵。

${\hat{X}}_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(m\right)}{\mathrm{\ξ}}_k^{j,r}---\left(13\right)$

其中，是模型j在k时刻的一步状态预测值；

$P_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j){({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j)}^T+{\mathrm{\Ψ}}_{k-1}^j{Q}_{k-1}^j{\left({\mathrm{\Ψ}}_{k-1}^j\right)}^T---\left(14\right)$

其中，为模型j在k时刻的一步预测误差方差矩阵。表示模型j在(k-1)时刻的自适应矩阵，的初始值由人为设定。

步骤4.3：通过公式(15)-(17)进行观测更新

${\hat{y}}_k^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(m\right)}h\left({\mathrm{\ξ}}_k^{j,r}\right)---\left(15\right)$

其中表示模型j在k时刻观测估计值，r表示第0个sigma点到第个sigma点的标识，h(·)为在步骤1中获取的观测方程中的非线性函数。

$P_{\mathrm{yy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]{[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T+R_k---\left(16\right)$

$P_{\mathrm{xy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}[{\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j]{[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T---\left(17\right)$

y为模型j在k时刻的时间更新误差协方差矩阵，为模型j在k时刻交叉误差协方差矩阵。

步骤4.4：通过公式(18)至公式(20)进行滤波更新

$K_k^j=P_{\mathrm{xy}}^j{\left(P_{\mathrm{yy}}^j\right)}^{-1}---\left(18\right)$

其中为模型j在k时刻的滤波增益矩阵。

${\hat{X}}_k^j={\hat{X}}_{k,k-1}^j+K_k^j(y_k-{\hat{y}}_k^j)---\left(19\right)$

为模型j中k时刻的状态估计值，为k时刻的该GPS***接收机的观测值，为已知量。

$P_k^j=P_{k,k-1}^j-K_k^j{P}_{\mathrm{yy}}^j{\left(K_k^j\right)}^T---\left(20\right)$

为k时刻模型j的估计误差方差矩阵。

步骤5：计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵

步骤5.1：用公式(21)至公式(23)计算模型j在k时刻的的估计偏差

${\mathrm{\υ}}_k^j=y_k^j-{\hat{y}}_k^j---\left(21\right)$

其中为模型j在k时刻的残差序列。残差序列的协方差矩阵用表示，用公式(22)计算。

$V_k^j=E\left[{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T\right]=\frac{1}{k-1}\underset{s=1}{\overset{s=k}{\mathrm{\Σ}}}{\mathrm{\υ}}_s^j{\left({\mathrm{\υ}}_s^j\right)}^T=\left\{\begin{array}{cc}{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=1\\ V_{k-1}^j+{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=2\\ \frac{k-2}{k-1}(V_{k-1}^j+\frac{{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T}{k-2}),& k\≥3\end{array}\right.---\left(22\right)$

其中，s表示时刻从1到k的变化指示。

模型j在k时刻的估计偏差矩阵用符号表示，并利用公式(23)求取。

${\mathrm{\Δ}}_k^j=\mathrm{trace}\left(V_k^j\right)-\mathrm{trace}\left(P_{\mathrm{yy}}^j\right)---\left(23\right)$

其中trace(·)表示对矩阵求迹。

步骤5.2：采用平滑滤波器求取模型j的过程噪声协方差矩阵的自适应因子

过程噪声协方差矩阵中起作用的主要为对角线上的值，因此设计自适应因子为对角矩阵，即

${\mathrm{\Ψ}}_k^j=\mathrm{diag}\{q_k^j\left(1\right),q_k^j\left(2\right),q_k^j\left(3\right),q_k^j\left(4\right),q_k^j\left(5\right),q_k^j\left(6\right),q_k^j\left(8\right),q_k^j\left(9\right),\mathrm{1,1}\}---\left(24\right)$

其中表示模型j在k时刻过程噪声协方差的自适应因子的对角线元素，用公式(25)和公式(26)求取。自适应矩阵中的后两项为1表示接收机钟差和钟差漂移的过程噪声协方差矩阵无校正。

$\left\{\begin{array}{c}{\stackrel{\‾}{q}}_k^j\left(l\right)={\mathrm{\λ}}_1{\stackrel{\‾}{q}}_{k-1}^j\left(l\right)+{\mathrm{\λ}}_2{\mathrm{\Δ}}_k({\mathrm{\Δ}}_k>\mathrm{\η})\\ \begin{array}{cc}{\stackrel{\‾}{q}}_k^j\left(l\right)=1& ({\mathrm{\Δ}}_k\≤\mathrm{\η})\end{array}\end{array}---\left(25\right)\right.$

$q_k^j\left(l\right)=\sqrt{{\stackrel{\‾}{q}}_k^j\left(l\right)}---\left(26\right)$

其中为求取对角线元素的中间变量。λ₁和λ₂为平滑滤波系数，且0＜λ₁＜1，λ₁+λ₂＝1。η为门限参数，其值由人为设定。

经过步骤4和步骤5的操作，完成了模型j的自适应UKF滤波。

步骤6：在步骤5操作基础上，通过公式(27)至公式(29)对模型概率更新。

$u_k^j=\frac{{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j}{c}---\left(27\right)$

其中，为模型j在k时刻的似然函数，可通过公式(28)计算得到；c为归一化常数，可通过公式(29)计算得到。

${\mathrm{\Λ}}_k^j\frac{1}{\sqrt{2\mathrm{\π}\left|P_{\mathrm{yy}}^j\right|}}\exp [-0.5{\left({\mathrm{\υ}}_k^j\right)}^T{\left(P_{\mathrm{yy}}^j\right)}^{-1}{\mathrm{\υ}}_k^j]---\left(28\right)$

其中exp[·]表示以自然常数e为底的指数函数。

$c=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j---\left(29\right)$

为模型j的归一化常数，π_ij为模型i到模型j的转移概率，其值由认为设定，为k-1时刻的模型概率，在0时刻由人为设定，并在以后每一时刻通过公式(27)更新。

步骤7：在步骤6操作基础上，进行信息融合,得到k时刻滤波器的输出。

${\hat{X}}_k=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{u}_k^j{\hat{X}}_k^j---\left(30\right)$

$P_k=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{u}_k^j[P_k^j+({\hat{X}}_k^j-{\hat{X}}_k){({\hat{X}}_k^j-{\hat{X}}_k)}^T]---\left(31\right)$

其中为采用交互式多模型自适应UKF(IMM-AUKF)算法得到的***最优估计值，P_k为采用IMM-AUKF算法得到的状态最优估计值的误差协方差矩阵。

步骤8：判断当前时刻是否到达步骤2设置的当前定位任务的结束时间，如果到达，则结束操作；否则，在步骤6操作基础上，将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值，重复执行步骤2至步骤7的操作，计算下一时刻滤波器的输出值。

有益效果

本发明提出的应用于卫星导航的自适应估计方法与以有技术相比较，其优势在于：针对卫星导航***中复杂机动载体运动模型和噪声统计特性未知导致定位精度低的问题从两个方面改善了载体定位定速的精度：首先利用残差序列的自协方差矩阵作为校正量，设计平滑滤波器将此校正量与陈旧自适应因子融合，得到一个新的自适应因子，并利用此自适应因子实时调整过程噪声协方差矩阵，有效地降低了***中噪声统计特性未知造成的定位误差。其次，采用交互式多模型算法(IMM)设置模型集M，并根据测量残差实时调整模型概率实现模型间的软切换，降低了***模型不精确造成的定位误差。

附图说明

图1为本发明具体实施方式中一种带自适应因子的交互式多模型UKF的卫星导航方法的流程示意图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实施实例对本发明做进一步说明。

本实施例要完成300秒时间内地面车载的定位任务。该车载在0至60秒时间内做匀速运动，在60至102秒时间内做匀速转弯运动，在103至162秒时间内做匀速直线运动，在183至195秒内做匀速转弯运动，196秒至205秒的时间内做匀加速直线运动，206秒至300秒时间内做匀减速直线运动。

使用本发明提出的应用于卫星导航的自适应估计方法进行定位，其流程如图1所示，具体步骤如下：

步骤1：建立***模型

步骤1.1：建立基于全球定位***(GPS)的地面载体的状态方程。

为全面描述载体的运动状态和钟差漂移频率，用符号X表示全球定位***(GPS)状态方程的状态变量， $X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x,}y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z,}{\mathrm{\δt}}_u,\mathrm{\δ}{\stackrel{\·}{t}}_u]}^T.$ 其中，分别代表世界大地坐标***(WorldGeodeticSystem一1984CoordinateSystem，WGS-84坐标系)下X_T轴上的位置、速度和加速度；分别代表WGS-84坐标系下Y_T轴上的位置、速度和加速度；分别代表WGS-84坐标系下Z_T轴上的位置、速度和加速度；δt_u和分别代表载体接收机钟差和钟差漂移频率。建立模型集M，模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT，即M＝[CS,CA,CT]。其中“当前”统计模型假设下一时刻的加速度为当前时刻的加速度加上一个有限值，即下一时刻加速度的取值是有限的，且只能在当前加速度的附近变化。“当前”统计模型CS对环境有较强的适应能力，是描述载体运动状态常用的模型之一；常加速模型CA加速在载体运动过程中，加速度的取值是常值；常速转弯模型CT假设载体的在一个平面内做角速度恒定的转弯运动。

接收机钟差变化率用一阶马尔科夫模型描述，当前统计模型CS的状态方程如公式(1)所示；常加速度模型CA的状态方程如公式(2)所示；常速转弯模型CT的状态方程如公式(3)所示。公式(1)中Φ₁＝diag{Φ_1,xΦ_1,yΦ_1,zΦ_t,u}其中

${\mathrm{\Φ}}_{1,x}={\mathrm{\Φ}}_{1,y}={\mathrm{\Φ}}_{1,z}=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\λ}}_{\mathrm{\τ}}^2}(-1+{\mathrm{\λ}}_{\mathrm{\τ}}T+e^{-{\mathrm{\λ}}_{\mathrm{\τ}}T})\\ 0& 1& \frac{1}{{\mathrm{\λ}}_{\mathrm{\τ}}}(1-e^{-{\mathrm{\λ}}_{\mathrm{\τ}}T})\\ 0& 0& e^{-{\mathrm{\λ}}_{\mathrm{\τ}}T}\end{array}\right],{\mathrm{\Φ}}_{t,u}=\left[\begin{array}{cc}1& (1-e^{-{\mathrm{\λ}}_{t}T})/{\mathrm{\λ}}_t\\ 0& e^{-{\mathrm{\λ}}_{t}T}\end{array}\right]$

其中λ_τ表示WGS-84坐标系下各个坐标轴上的加速度时间常数的倒数，取为0.1，T为采样时间间隔，取为1；λ_t为一阶马尔科夫时间常数倒是，取为0.1。

公式(2)中Φ₂＝diag{Φ_2,xΦ_2,yΦ_2,zΦ_t,u}，其中 ${\mathrm{\Φ}}_{2,x}={\mathrm{\Φ}}_{2,y}={\mathrm{\Φ}}_{2,z}=\left[\begin{array}{ccc}1& T& {0.5T}^2\\ 0& 1& T\\ 0& 0& 1\end{array}\right].$

公式(3)中，

Φ₃＝diag{Φ_3,xΦ_3,yΦ_3,zΦ_t,u}，其中 ${\mathrm{\Φ}}_{3,x}={\mathrm{\Φ}}_{3,y}={\mathrm{\Φ}}_{3,z}=\left[\begin{array}{ccc}1& \frac{\sin \mathrm{\ωT}}{\mathrm{\ω}}& \frac{(1-\cos \mathrm{\ωT})}{{\mathrm{\ω}}^2}\\ 0& \cos \mathrm{\ωT}& \frac{\sin \mathrm{\ωT}}{\mathrm{\ω}}\\ 0& -\mathrm{\ω}\sin \mathrm{\ωT}& \cos \mathrm{\ωT}\end{array}\right],$ ω代表载体的角度度，设为0.2。

用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k-1)时刻的过程噪声协方差矩阵， E[·]表示期望值运算，表示WGS-84坐标系下描述X_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Y_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Z_T轴上载体运动状态的过程噪声协方差矩阵；Q_t,k-1表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵；

$Q_{k-1}^1=\mathrm{diag}\{Q_{x,k-1}^1,Q_{y,k-1}^1,Q_{z,k-1}^1,Q_{t,k-1}\},$

$Q_{x,k-1}^1=Q_{y,k-1}^1=Q_{z,k-1}^1={2\mathrm{\λ}}_{\mathrm{\τ}}\left[\begin{array}{ccc}\frac{T^5}{20}& \frac{T^4}{8}& \frac{T^3}{6}\\ \frac{T^4}{8}& \frac{T^3}{6}& \frac{T^2}{2}\\ \frac{T^3}{6}& \frac{T^2}{2}& T\end{array}\right]Q_{t,k-1}=\left[\begin{array}{cc}\frac{T^3}{3}& \frac{T^2}{2}\\ \frac{T^2}{2}& T\end{array}\right];$

$Q_{k-1}^2=\mathrm{diag}\{Q_{x,k-1}^2,Q_{y,k-1}^2,Q_{z,k-1}^2,Q_{t,k-1}\},$ 其中 $Q_{x,k-1}^2=Q_{y,k-1}^2=Q_{z,k-1}^2=\left[\begin{array}{ccc}\frac{T^4}{20}& \frac{T^3}{8}& \frac{T^2}{6}\\ \frac{T^3}{8}& \frac{T^2}{3}& \frac{T}{2}\\ \frac{T^2}{6}& \frac{T}{2}& 1\end{array}\right];$

$Q_{k-1}^3=\mathrm{diag}\{Q_{x,k-1}^3,Q_{y,k-1}^3,Q_{z,k-1}^3,Q_{t,k-1}\},$

其中，

$Q_{x,k1}^3=Q_{y,k-1}^3=Q_{z,k-1}^3=\left[\begin{array}{ccc}\frac{6\mathrm{\ωT}-8\sin \mathrm{\ωT}+\sin 2\mathrm{\ωT}}{{4\mathrm{\ω}}^5}& \frac{{2\sin }^4(\mathrm{\ωT}/2)}{{\mathrm{\ω}}^4}& \frac{-2\mathrm{\ωT}+4\sin \mathrm{\ωT}-\sin 2\mathrm{\ωT}}{{4\mathrm{\ω}}^3}\\ \frac{{2\sin }^4(\mathrm{\ωT}/2)}{{\mathrm{\ω}}^4}& \frac{2\mathrm{\ωT}-\sin 2\mathrm{\ωT}}{{4\mathrm{\ω}}^3}& \frac{{\sin }^2\mathrm{\ωT}}{{2\mathrm{\ω}}^2}\\ \frac{-2\mathrm{\ωT}+4\sin T-\sin 2\mathrm{\ωT}}{{4\mathrm{\ω}}^3}& \frac{{\sin }^2\mathrm{\ωt}}{{2\mathrm{\ω}}^2}& \frac{2\mathrm{\ωT}+\sin 2\mathrm{\ωT}}{4\mathrm{\ω}}\end{array}\right.$

步骤1.2：建立基于全球定位***(GPS)的地面载体的观测方程

采用伪距作为观测量建立观测方程。在1秒至300秒内，接收机观测到的5颗GPS卫星，并利用这5颗GPS卫星进行定位，则建立此时刻CS模型，CA模型和CT模型的伪距观测方程如公式(4)所示。其中公式(4)中n的取值范围为n＝1,2,3,4,5

为下文描述算法方便，建立CS模型，CA模型和CT模型的观测方程为公式(5)。

用R_k表示公式(5)的观测误差的协方差矩阵，

步骤2：设置CS模型，CA模型和CT模型0时刻的滤波初始值，其中位置、速度和接收机钟差的初始值由最小二乘法获得，加速度和接收机钟差变化量的初值设为0；同时设置当前定位任务的结束时间为300秒。

步骤3：得到CS模型，CA模型和CT模型在k时刻的滤波初始值。

使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k-1)时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合，分别得到CS模型，CA模型和CT模型中k时刻的状态的估计初值和以及CS模型，CA模型和CT模型中k时刻的估计误差协方差矩阵初值和

步骤4：对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波(AdaptiveUnsignedKalmanFilter,AUKF)，得到各模型在k时刻的滤波值。具体操作步骤为：

步骤4.1：用表示步骤1中得到的CS模型，CA模型和CT模型的状态方程中的维数，通过公式(8)计算模型j在k时刻个初始西格马(sigma)点；

用公式(9)计算第一类权系数该权系数用于求每个sigma点的一阶统计特性；用公式(10)计算第二类权系数该权系数用于求每个sigma点的二阶统计特性；其中公式(9)和(10)中的β取值为2，α取值为0.01，公式(11)中的参数κ取0。

步骤4.2：通过公式(12)至公式(14)进行时间更新。

其中公式(14)中的初始值设为单位矩阵。

步骤4.3：通过公式(15)-(17)进行观测更新

步骤4.4：通过公式(18)至公式(20)进行滤波更新

步骤5：计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵

步骤5.1：用公式(21)至公式(22)计算模型j在k时刻的残差序列的协方差矩阵。并利用公式(23)求模型j在k时刻的估计偏差矩阵。

步骤5.2：采用公式(25)设计的平滑滤波器求取模型j的过程噪声协方差矩阵的自适应矩阵

通过公式(25)至公式(26)设计的平滑滤波器实现Δ_k与陈旧自适应因子的融合。

公式(26)中平滑滤波系数设置为λ₁＝0.98，λ₂＝0.02，门限参数设置为10。

经过步骤4和步骤5的操作，完成了模型j的自适应UKF滤波，j＝1,2,3,分别表示CS模型，CA模型和CT模型。

步骤6：在步骤5操作基础上，通过公式(27)至公式(29)对模型概率更新

其中模型i到模型j的转移概率π_ij设置为 $\mathrm{\π}=\left[\begin{array}{ccc}0.98& 0.01& 0.01\\ 0.01& 0.98& 0.01\\ 0.01& 0.01& 0.98\end{array}\right].$

步骤7：在步骤6操作基础上，通过公式(30)和公式(31)进行信息融合,得到k时刻滤波器的输出。

步骤8：判断当前时刻是否到达步骤2设置的当前定位任务的结束时间300秒，如果到达，则结束操作；否则，在步骤6操作基础上，将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值，重复执行步骤2至步骤7的操作，计算下一时刻滤波器的输出值。

为了说明本发明的效果，分别使用已有的UKF算法、IMM-UKF以及本发明提出的IMM-AUKF方法对实施例中的定位任务进行定位，得到的误差对表结果如表1所示。

表1UKF算法、IMM-UKF和IMM-AUKF算法的实验对比结果

从表1中可以看出，与传统方法相比，本发明提出的IMM-AUKF方法的定位精度最高。

Claims (3)

1.一种带自适应因子的交互式多模型UKF的卫星导航方法，其特征在于：具体包括如下步骤：

步骤1：建立***模型；

步骤1.1：建立基于全球定位***GPS的地面载体的状态方程；

为全面描述载体的运动状态和钟差漂移频率，用符号X表示全球定位***GPS状态方程的状态变量，其中，分别代表WGS-84坐标系下X_T轴上的位置、速度和加速度；分别代表WGS-84坐标系下Y_T轴上的位置、速度和加速度；分别代表WGS-84坐标系下Z_T轴上的位置、速度和加速度；δt_u和分别代表载体接收机钟差和钟差漂移频率；建立模型集M，模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT，即M＝[CS,CA,CT]；接收机钟差变化率用一阶马尔科夫模型描述，当前统计模型CS的状态方程如公式(1)所示；常加速度模型CA的状态方程如公式(2)所示；常速转弯模型CT的状态方程如公式(3)所示；

$X_k^1={\mathrm{\Φ}}_1{X}_{k-1}^1+{\mathrm{\Ψ}}_{k-1}^1{w}_{k-1}^1---\left(1\right)$

$X_k^2={\mathrm{\Φ}}_2{X}_{k-1}^2+{\mathrm{\Ψ}}_{k-1}^2{w}_{k-1}^2---\left(2\right)$

$X_k^3={\mathrm{\Φ}}_3{X}_{k-1}^3+{\mathrm{\Ψ}}_{k-1}^3{w}_{k-1}^3---\left(3\right)$

其中，分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态，其中i＝1,2,3，即：表示***在当前统计模型CS下k时刻的状态；表示***在常加速度模型CA下k时刻的状态；表示***在常速转弯模型CT下k时刻的状态；k为正整数；Φ_i为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵，Φ_i＝diag(Φ_i,x,Φ_i,y,Φ_i,z,Φ_t,u)，Φ_i,x表示WGS-84坐标系下X_T轴上的状态转移矩阵；Φ_i,y表示WGS-84坐标系下Y_T轴上的状态转移矩阵；Φ_i,z表示WGS-84坐标系下Z_T轴上的状态转移矩阵；Φ_t,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵；是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子；是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵；

用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k-1时刻的过程噪声协方差矩阵， i＝1,2,3，E[·]表示期望值运算，表示WGS-84坐标系下描述X_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Y_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Z_T轴上载体运动状态的过程噪声协方差矩阵；Q_t,k-1表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵；

步骤1.2：建立基于全球定位***GPS的地面载体的观测方程；

采用伪距作为观测量建立观测方程；在k时刻，接收机观测到的GPS卫星数量用N表示，则建立此时刻CS模型，CA模型和CT模型的伪距观测方程如公式(4)所示；

${\mathrm{\ρ}}_n^k=\sqrt{{(x_k^i-x_n^k)}^2+{({\stackrel{\‾}{y}}_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k+{\mathrm{\ϵ}}_n^k---\left(4\right)$

其中，表示在k时刻观测到的第n颗GPS卫星的伪距，n＝1,2…N，N表示k时刻观测到的GPS卫星的总数；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下X_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Y_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Z_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下X_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Y_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Z_T轴上的位置；表示k时刻地面载体接收机钟差；表示k时刻观测到的第n颗GPS卫星的观测误差；

为下文描述算法方便，建立CS模型，CA模型和CT模型的观测方程为公式(5)；

$y_k^i=h\left(X_k^i\right)+{\mathrm{\ϵ}}_k---\left(5\right)$

其中，为***的观测量，观测方程中的非线性函数，为***的观测误差；

用R_k表示公式(5)的观测误差的协方差矩阵，

步骤2：设置CS模型，CA模型和CT模型0时刻的滤波初始值，同时设置当前定位任务的结束时间；

在步骤1操作的基础上，分别设置CS模型、CA模型和CT模型的状态的估计初值，用符号和表示；然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值，用符号和表示；并设置***运行的结束时间；

步骤3：得到CS模型，CA模型和CT模型在k时刻的滤波初始值；

使用CS模型、CA模型和CT模型第k-1时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合，分别得到CS模型，CA模型和CT模型中k时刻的状态的估计初值，分别用和表示，以及CS模型，CA模型和CT模型中k时刻的估计误差协方差矩阵初值，分别用和表示；

${\hat{X}}_{k-1}^{0j}=\underset{i=1}{\overset{3}{\Σ}}{\hat{X}}_{k-1}^i{u}_{k-1|k-1}^{i|j}---\left(6\right)$

其中，分别表示CS模型，CA模型和CT模型在k时刻滤波的状态初值；j＝1,2,3，分别代表CS模型、CA模型和CT模型；i＝1,2,3，分别表示CS模型，CA模型和CT模型在k-1时刻状态的最优估计值；表示k-1时刻模型i和模型j的混合概率，π_ij为模型i到模型j的模型转移概率，其值由人为设定；为模型i在k时刻的概率，的初值由人为设定，并在以后各个时刻由步骤6更新；代表模型j的归一化常数，

$P_{k-1}^{0j}=\underset{i=1}{\overset{3}{\Σ}}{u}_{k-1|k-1}^{i|j}\[P_{k-1}^i+({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j}){({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j})}^T\]---\left(7\right)$

其中，i＝1,2,3，分别表示CS模型、CA模型和CT模型在k-1时刻滤波误差的方差矩阵；分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值；

步骤4：对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF，得到各模型在k时刻的滤波值；

步骤5：计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵步骤6：在步骤5操作基础上，通过公式(8)至公式(10)对模型概率更新；

$u_k^j=\frac{{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j}{c}---\left(8\right)$

其中，为模型j在k时刻的似然函数，可通过公式(9)计算得到；c为归一化常数，可通过公式(10)计算得到；

${\mathrm{\Λ}}_k^j=\frac{1}{\sqrt{2\mathrm{\π}\left|P_{yy}^j\right|}}\exp \[-0.5{\left({\mathrm{\υ}}_k^j\right)}^T{\left(P_{yy}^j\right)}^{-1}{\mathrm{\υ}}_k^j\]---\left(9\right)$

其中exp[·]表示以自然常数e为底的指数函数；

$c=\underset{j=1}{\overset{3}{\Σ}}{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j---\left(10\right)$

为模型j的归一化常数，π_ij为模型i到模型j的转移概率，其值由人为设定，为k-1时刻的模型概率，在0时刻由人为设定，并在以后每一时刻通过公式(8)更新；

步骤7：在步骤6操作基础上，进行信息融合,得到k时刻滤波器的输出；

${\hat{X}}_k=\underset{j=1}{\overset{3}{\Σ}}{u}_k^j{\hat{X}}_k^j---\left(11\right)$

$P_k=\underset{j=1}{\overset{3}{\Σ}}{u}_k^j\[P_k^j+({\hat{X}}_k^j-{\hat{X}}_k){({\hat{X}}_k^j-{\hat{X}}_k)}^T\]---\left(12\right)$

其中为采用交互式多模型自适应UKF算法得到的***最优估计值，P_k为采用IMM-AUKF算法得到的状态最优估计值的误差协方差矩阵；

步骤8：判断当前时刻是否到达步骤2设置的当前定位任务的结束时间，如果到达，则结束操作；否则，在步骤6操作基础上，将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值，重复执行步骤2至步骤7的操作，计算下一时刻滤波器的输出值。

2.如权利要求1所述的一种带自适应因子的交互式多模型UKF的卫星导航方法，其特征在于：其步骤4中所述对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF，得到各模型在k时刻的滤波值的具体操作步骤为：

步骤4.1：用表示***状态的维数，即步骤1中得到的CS模型，CA模型和CT模型中状态方程的维数，通过公式(13)计算模型j在k时刻个初始sigma点，其中第r个sigma点用用符号表示，且r为整数；

其中，表示平方根矩阵的第r列或第r行；

用公式(14)计算第一类权系数，该权系数用于求每个sigma点的一阶统计特性；模型j的第r个sigma点的第一类权系数用符号表示，即：表示CS模型的第r个sigma点第一类权系数，表示CA模型的第r个sigma点第一类权系数，表示CT模型的第r个sigma点第一类权系数；用公式(15)计算第二类权系数，该权系数用于求每个sigma点的二阶统计特性；模型j的第r个sigma点的第二类权系数用符号表示，即：表示CS模型的第r个sigma点第二类权系数，表示CA模型的第r个sigma点第二类权系数，表示CT模型的第r个sigma点第二类权系数；

其中，λ为一个复合刻度参数，其值通过公式(16)求解，参数α决定sigma点的散布程度，其值由人为设定，1e^-4≤α≤1；参数β用来描述全球定位***状态方程的状态变量X的分布信息，在高斯情况下，β的最优值为2；

$\mathrm{\λ}={\mathrm{\α}}^2(\tilde{n}+\mathrm{\κ})-\tilde{n}---\left(16\right)$

其中，参数κ取0；

步骤4.2：通过公式(17)至公式(19)进行时间更新；

${\mathrm{\ξ}}_k^{j,r}={\mathrm{\Φ}}_j{\mathrm{\ξ}}_{k-1}^{j,r}---\left(17\right)$

其中，表示模型j在k时刻经过一步转移后的第r个sigma点；Φ_j为模型j的状态转移矩阵；

${\hat{X}}_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\Σ}}{w}_{j,r}^{\left(m\right)}{\mathrm{\ξ}}_k^{j,r}---\left(18\right)$

其中，是模型j在k时刻的一步状态预测值；

$P_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\Σ}}{w}_{j,r}^{\left(c\right)}({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j){({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j)}^T+{\mathrm{\Ψ}}_{k-1}^j{Q}_{k-1}^j{\left({\mathrm{\Ψ}}_{k-1}^j\right)}^T---\left(19\right)$

其中，为模型j在k时刻的一步预测误差方差矩阵；表示模型j在k-1时刻的自适应矩阵，的初始值由人为设定；

步骤4.3：通过公式(20)至(22)进行观测更新；

${\hat{y}}_k^j=\underset{r=0}{\overset{2\tilde{n}}{\Σ}}{w}_{j,r}^{\left(m\right)}h\left({\mathrm{\ξ}}_k^{j,r}\right)---\left(20\right)$

其中表示模型j在k时刻观测估计值，r表示第0个sigma点到第个sigma点的标识，h(·)为在步骤1中获取的观测方程中的非线性函数；

$P_{yy}^j=\underset{r=0}{\overset{2\tilde{n}}{\Σ}}{w}_{j,r}^{\left(c\right)}\[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j\]{\[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j\]}^T+R_k---\left(21\right)$

$P_{xy}^j=\underset{r=0}{\overset{2\tilde{n}}{\Σ}}{w}_{j,r}^{\left(c\right)}\[{\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j\]{\[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j\]}^T---\left(22\right)$ 为模型j在k时刻的时间更新误差协方差矩阵，为模型j在k时刻交叉误差协方差矩阵；

步骤4.4：通过公式(23)至公式(25)进行滤波更新

$K_k^j=P_{xy}^j{\left(P_{yy}^j\right)}^{-1}---\left(23\right)$

其中为模型j在k时刻的滤波增益矩阵；

${\hat{X}}_k^j={\hat{X}}_{k,k-1}^j+K_k^j(y_k-{\hat{y}}_k^j)---\left(24\right)$

为模型j中k时刻的状态估计值，y_k为k时刻的该GPS***接收机的观测值，为已知量；

$P_k^j=P_{k,k-1}^j-K_k^j{P}_{yy}^j{\left(K_k^j\right)}^T---\left(25\right)$

为k时刻模型j的估计误差方差矩阵。

3.如权利要求1或2所述的一种带自适应因子的交互式多模型UKF的卫星导航方法，其特征在于：其步骤5中所述计算模型j在k时刻的过程噪声协方差矩阵的自适应矩阵的具体操作步骤为：

步骤5.1：用公式(26)至公式(28)计算模型j在k时刻的的估计偏差

${\mathrm{\υ}}_k^j=y_k^j-{\hat{y}}_k^j---\left(26\right)$

其中为模型j在k时刻的残差序列；残差序列的协方差矩阵用表示，用公式(27)计算；

$V_k^j=E\[{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T\]=\frac{1}{k-1}\underset{s=1}{\overset{s=k}{\Σ}}{\mathrm{\υ}}_s^j{\left({\mathrm{\υ}}_s^j\right)}^T=\left\{\begin{array}{cc}{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=1\\ V_{k-1}^j+{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=2\\ \frac{k-2}{k-1}(V_{k-1}^j+\frac{{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T}{k-2}),& k\≥3\end{array}\right.---\left(27\right)$

其中，s表示时刻从1到k的变化指示；

模型j在k时刻的估计偏差矩阵用符号表示，并利用公式(28)求取；

${\mathrm{\Δ}}_k^j=trace\left(V_k^j\right)-trace\left(P_{yy}^j\right)---\left(28\right)$

其中trace(·)表示对矩阵求迹；

步骤5.2：采用平滑滤波器求取模型j的过程噪声协方差矩阵的自适应因子

过程噪声协方差矩阵中起作用的主要为对角线上的值，因此设计自适应因子为对角矩阵，即

${\mathrm{\Ψ}}_k^j=diag\{q_k^j\left(1\right),q_k^j\left(2\right),q_k^j\left(3\right),q_k^j\left(4\right),q_k^j\left(5\right),q_k^j\left(6\right),q_k^j\left(7\right),q_k^j\left(8\right),q_k^j\left(9\right),1,1\}---\left(29\right)$

其中表示模型j在k时刻过程噪声协方差的自适应因子的对角线元素，用公式(30)和公式(31)求取；自适应矩阵中的后两项为1表示接收机钟差和钟差漂移的过程噪声协方差矩阵无校正；

$\left\{\begin{array}{cc}{\stackrel{\‾}{q}}_k^j\left(l\right)={\mathrm{\λ}}_1{\stackrel{\‾}{q}}_{k-1}^j\left(l\right)+{\mathrm{\λ}}_2{\mathrm{\Δ}}_k& (\mathrm{\Δ}k>\mathrm{\η})\\ {\stackrel{\‾}{q}}_k^j\left(l\right)=1& (\mathrm{\Δ}k\≤\mathrm{\η})\end{array}\right.---\left(30\right)$

$q_k^j\left(l\right)=\sqrt{{\stackrel{\‾}{q}}_k^j\left(l\right)}---\left(31\right)$

其中为求取对角线元素的中间变量；λ₁和λ₂为平滑滤波系数，且0＜λ₁＜1，λ₁+λ₂＝1；η为门限参数，其值由人为设定。