CN106772524A - 一种基于秩滤波的农业机器人组合导航信息融合方法 - Google Patents

Abstract

本发明公开了一种基于秩滤波的农业机器人组合导航信息融合方法，采用交互式多模型滤波为算法结构，通过上一个时刻各个模型滤波器的输出结果计算当前时刻滤波的初值，将秩滤波器作为模型滤波步骤中的子滤波器进行并行滤波，利用模型滤波中得到的残差、残差协方差对模型概率进行更新，对各个秩滤波器估值进行加权融合从而得到交互输出结果。由导航计算机根据组合导航系统的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明能够提高复杂环境下组合导航系统的滤波精度，使得农业机器人具有适应范围广和抗干扰能力强的优点。

Description

一种基于秩滤波的农业机器人组合导航信息融合方法

技术领域

本发明属于多传感器信息融合技术领域，尤其涉及一种基于秩滤波的农业机器人组合导航信息融合方法。

背景技术

农业机器人工作在复杂的开放式非结构化农田环境中(障碍物、土地松软程度不一高低不平、遮挡、光照变化等)，它需要通过自身携带的传感器感知周围的环境，对感知到的信息进行处理，获取自身的位置和姿态信息，然后实时进行路径规划和导航控制，完成各项作业任务。因此，导航技术是实现农业机器人自动化作业的关键。Kalman滤波器一般适用于系统为线性、噪声统计特性服从高斯分布并完全已知的情况；传统的扩展卡尔曼滤波(extended Kalman filter，EKF)及其改进方法只能处理弱非线性系统及高斯噪声条件下的状态估计问题。为此，人们提出了一系列以贝叶斯滤波理论为框架、基于数值积分近似的非线性状态估计方法，其中具有代表性的方法包括中心差分卡尔曼滤波、无迹卡尔曼滤波、求积分卡尔曼滤波、容积卡尔曼滤波等。它们的共同特点是均假设系统中噪声服从高斯分布，即为高斯滤波器。秩滤波器(rank Kalman filter,RKF)具有与高斯滤波器相同的滤波结构，它通过秩统计量原理确定采样点和权值，因此还适用于具有非高斯噪声的系统。交互式多模型(Interacting Multiple Model,IMM)算法以广义伪贝叶斯算法为基础，利用Markov切换系数实现各个模型之间的交互，兼具计算复杂度低和估计精度高的特点。

发明内容

本发明提出了一种基于秩滤波的农业机器人组合导航信息融合方法，该方法的主要思路是在交互式多模型滤波框架下，采用秩滤波器替代传统算法中的卡尔曼滤波器，突破传统算法中系统噪声必须为高斯分布的条件限制，增强各个子滤波器状态估计的稳定性，提高复杂环境下组合导航系统的滤波精度，增强农业机器人自主导航定位的性能。

本发明采用交互式多模型滤波为算法结构，将秩滤波器作为模型滤波步骤中的子滤波器进行并行滤波，本发明具体包括如下步骤：

步骤1，根据农业机器人所处环境，将土地平整度和卫星信号强度分级，设定土地平整度因子和卫星信号强度因子ξ，1≤ξ≤ξ_max，ξ∈Z，Z为整数集，ξ_max分别表示最大的土地不平整级别和最大的卫星信号衰落级别；

步骤2，由惯性导航系统INS和全球卫星导航系统GNSS所测量的数据，测量各级别土地平整度和卫星信号衰落情形所对应的量测噪声ν，将量测噪声ν及其协方差矩阵R作为可变参数建立组合导航系统模型集合M，设模型个数为r，各模型之间跳变的Markov转移概率为π_ji，下标i，j分别表示第i个模型和第j个模型；

步骤3，由k-1时刻的模型概率与先验的Markov转移概率π_ji进行交互，计算混合概率k≥1；

步骤4，对于第j个模型，j＝1,2,…，r，利用k-1时刻的状态估计和协方差阵计算k时刻的状态初值与协方差阵初值

步骤5，在组合导航系统获得新的量测z^k之后，利用状态初值与协方差阵初值采用秩滤波算法进行滤波，得到k时刻的状态估计和协方差阵

步骤6，利用混合概率残差估值和残差协方差计算当前时刻的匹配模型对应的残差估值残差协方差似然函数和模型概率

步骤7，利用各个模型滤波器k时刻的状态估计协方差阵和模型概率计算导航参数的全局估计及估计误差的协方差阵P_k|k；

步骤8，返回步骤3，重复步骤3至步骤7，直至接收到停止导航控制指令；

本发明步骤3中，通过如下公式计算混合概率

本发明步骤4中，通过如下公式计算k时刻的状态初值与协方差阵初值

本发明步骤5包括如下步骤：

步骤5-1，利用k-1时刻的状态估计一维标准分布概率p_β对应的分位点和第β层采样点的修正系数r_β，通过秩采样机制求取采样点集

其中，n为状态向量x的维数，ρ为采样点层数，α，β分别表示第α个采样点和第β层采样点，l取值为[1,n]中的整数，表示矩阵平方根的第l列；

步骤5-2，将采样点集进行非线性传播，得到非线性变换后点集

其中，f(·)为状态方程非线性函数，ω为过程噪声向量；

步骤5-3，进一步预测k时刻的状态估计和协方差阵

其中，为第α个采样点对应的协方差修正系数，取值为1，Q为过程噪声方差矩阵，τ为协方差权重系数，通过如下公式计算：

步骤5-4，通过秩采样机制求取采样点集

步骤5-5，将采样点集引入观测方程并进行非线性传播，得到预测的量测点集

其中，h(·)为观测方程非线性函数，ν^j为模型j对应的量测噪声向量；

步骤5-6，通过预测的量测点集计算量测估计

步骤5-7，通过如下公式计算新息协方差矩阵和互协方差阵

其中R^j为模型j对应的量测噪声方差矩阵；

步骤5-8，由新息协方差矩阵和互协方差阵计算增益

k时刻组合导航系统的量测值z_k到来之后，通过如下公式计算k时刻的状态估计和协方差阵

本发明步骤6中，通过如下方法计算当前时刻匹配模型对应的残差估值残差协方差似然函数和模型概率

其中E[·]表示数学期望。

本发明步骤7中，通过如下公式计算导航参数的全局估计及其估计误差的协方差阵P_k|k：

本发明步骤5-1和步骤5-4中，采用中位秩计算第β层采样点对应的一维标准分布概率p_β，计算表达式为p_β＝(β－0.3)/(2ρ+1.4)。

本发明针对现有技术的不足，将秩滤波器与交互式多模型算法相结合，解决非线性、非高斯系统中的状态估计问题，提高了农业机器人组合导航算法估计性能。本发明内容同样适用于惯性组合导航、目标跟踪与识别、图像处理等其他多传感器信息融合及多源数据处理应用领域。

有益效果：本发明相对于现有技术的优点为：

(1)本发明采用交互式多模型信息融合算法结构，通过设计多个模型来逼近系统复杂的时变或非线性过程，能够使在建模条件下分析得到的系统性能保持或接近最优，相比于广义伪贝叶斯算法，兼具了计算复杂度低和估计精度高的优点。

(2)本发明将秩滤波器引入到交互式多模型信息融合算法中，秩滤波器通过秩统计量相关原理确定采样点和权值，因此，它不受高斯分布条件限制，对于未知噪声干扰具有更好的适应性，从而使组合导航系统具有更好的稳定性和抗干扰能力。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述或其他方面的优点将会变得更加清楚。

图1基于秩滤波的交互式多模型算法结构图；

图2农业机器人仿真运行轨迹；

图3俯仰角估计误差曲线；

图4横摇角估计误差曲线；

图5航向角估计误差曲线；

图6东向速度估计误差曲线；

图7北向速度估计误差曲线；

图8纬度估计误差曲线；

图9经度估计误差曲线。

具体实施方式

下面结合附图对发明的技术内容进行详细说明：

本发明的基于秩滤波的农业机器人组合导航信息融合方法，以交互式多模型算法为滤波框架，采用秩滤波器替代传统算法中的卡尔曼滤波器，如图1所示为基于秩滤波的交互式多模型算法结构图。现以微电子机械(Micro-Electro-Mechanical System,MEMS)捷联惯性导航系统和实时动态(Real-time kinematic,RTK)全球定位系统RTK GPS构成的农业机器人组合导航系统为例来说明所提供的信息融合算法，具体步骤如下：

步骤1，根据农业机器人所处环境，将土地平整度和卫星信号强度分级，设定土地平整度因子和卫星信号强度因子ξ(1≤ξ≤ξ_max，ξ∈Z),Z为整数集，ξ_max分别表示最大的土地不平整级别和卫星信号衰落级别；

步骤2，以东北天坐标系为导航坐标系，右前上坐标系为载体坐标系，根据先验知识与农业机器人所处环境(例如障碍物、土地平整度、遮挡、光照等)，建立MEMS捷联惯性导航系统、RTK GPS组合导航系统模型：

其中，状态向量X＝[φ_E φ_N φ_U δV_E δV_N δL δλ ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T，φ_E、φ_N、φ_U为东向、北向和天向失准角；δV_E、δV_N为东向、北向速度误差；δL、δλ为纬度、经度误差；ε_bx、ε_by、ε_bz为陀螺的常值漂移；▽_bx、▽_by、▽_bz为加速度计常值漂移。以MEMS-INS和GPS测得的速度、位置之差作为观测量，V_INS和V_GNSS为东北天坐标系下MEMS-INS和GPS测得的速度，P_INS和P_GNSS为东北天坐标系下MEMS-INS和GPS测得的位置；F和H分别为状态转移矩阵、观测矩阵；ω为状态噪声向量，协方差为Q；为量测噪声向量，考虑农田复杂环境对传感器测量精度的影响，建立以量测噪声向量ν及其协方差矩阵R为变化参数的模型集合M，设模型个数为r，Markov转移概率为π_ji，下标i，j分别表示第i个模型和第j个模型。

步骤3，由k-1(k≥1)时刻的模型概率与先验的Markov转移概率π_ji进行交互，计算混合概率

步骤4，对于第j＝1,2,…，r个模型，利用k-1时刻的状态估计和协方差阵计算k时刻的状态初值与协方差阵初值

步骤5，在组合导航系统获得新的量测z^k之后，利用步骤4中计算得到的状态初值与协方差阵初值采用秩滤波算法进行滤波，得到k时刻的状态估计协方差阵残差估值以及残差协方差具体流程为：

步骤5-1，以α，β为下标分别表示第α个采样点和第β层采样点，利用k-1时刻的状态估计一维标准分布概率p_β对应的分位点和第β层采样点的修正系数r_β，通过秩采样机制求取采样点集

其中，n为状态向量x的维数，ρ为采样点层数，l取值为[1,n]中的整数，表示矩阵平方根的第l列，第β层采样点对应的概率p_β采用中位秩计算得到，计算表达式为p_β＝(β－0.3)/(2ρ+1.4)。

步骤5-2，将采样点集进行非线性传播，得到非线性变换后点集

其中，f(·)为状态方程非线性函数，ω为过程噪声向量。

步骤5-3，进一步预测状态估计和协方差阵

其中，为第α个采样点对应的协方差修正系数，取值为1；Q为过程噪声方差矩阵；τ为协方差权重系数，其计算公式为：

步骤5-4，通过秩采样机制求取采样点集

其中，第β层采样点对应的概率p_β采用中位秩计算得到，计算表达式为p_β＝(β－0.3)/(2ρ+1.4)。

步骤5-5，将采样点集引入观测方程并进行非线性传播，得到预测的量测点集

其中，h(·)为观测方程非线性函数，ν^j为模型j对应的量测噪声向量。

步骤5-6，通过预测的量测点集计算量测估计

步骤5-7，计算新息协方差矩阵和互协方差阵

其中R^j为模型j对应的的量测噪声方差矩阵。

步骤5-8，由新息协方差矩阵和互协方差阵计算增益

k时刻组合导航系统的量测值z_k到来之后，计算k时刻的状态估计和协方差阵具体计算公式为：

步骤6，通过如下方法计算当前时刻匹配模型对应的残差估值残差协方差似然函数和模型概率：

步骤7，利用步骤5中各模型滤波器所得到的状态估计协方差阵和步骤6中获取的模型概率计算融合输出结果：

步骤8，返回步骤3，重复步骤3至步骤7，直至接收到停止导航控制指令。

实施例

为验证本发明的可行性，在Matlab下进行了仿真，滤波初始值及仿真参数设置如下：

陀螺随机常值漂移为1°/h，白噪声随机漂移为0.1°/h；

加速度计偏置误差为1mg，白噪声随机漂移为0.1mg；

MEMS捷联惯性导航系统初始水平姿态角误差为1°，航向角误差为3°；

初始速度为0m/s，初始速度误差为0m/s；

初始位置为东经118°，北纬32°，高度100m，初始位置误差为0m；

RTK GPS位置测量精度为0.1米；

MEMS捷联惯性导航系统采样周期为1ms，RTK GPS采样周期为0.1s；

最大的土地不平整级别最大卫星信号衰落级别ξ_max＝1，则建立的模型集合M中模型个数r为3，Markov转移概率为

仿真时间为3600s，假设农业机器人在农田中模拟“割草机”运动，轨迹如图2所示。在同等条件下，导航计算机根据传感器数据和系统模型，分别采用交互式多模型扩展卡尔曼滤波(IMM-EKF)、交互式多模型无迹卡尔曼滤波(IMM-UKF)和本发明提出的交互式多模型秩滤波(IMM-RKF)算法进行滤波估计，对比三种算法在组合导航系统中的估计结果。图3为俯仰角估计误差曲线，图4为横摇角估计误差曲线图，图5为航向角估计误差曲线，可以看出，三种算法所得到的水平姿态角误差均能收敛到很小的值并且收敛速度较快，航向角误差的修正效果不是很明显，但也能看出逐渐收敛的趋势，收敛速度较慢。从航向角误差的收敛过程可以看出，本发明提出的IMM-RKF算法估计效果优于其它两种算法；图6、图7分别为东向、北向速度估计误差曲线，由于实时动态GPS提供的速度信息对SINS的速度误差进行修正，采用IMM-RKF算法速度误差基本保持在0.2m/s以内，明显优于其它两种算法；图8、图9分别为纬度、经度估计误差曲线，可以看出IMM-UKF与IMM-RKF算法位置误差较为接近，IMM-EKF算法误差较大。进一步的，将三种算法得到的位置估计误差进行数值比较，采用IMM-EKF算法得到的经度估计误差均方差为0.051m，最大达到0.095m；纬度估计误差均方差为0.045m，最大达到0.102m。采用IMM-UKF算法得到的经度估计误差为0.042m，最大为0.049m；纬度估计误差均方差为0.008m，最大为0.061m。采用IMM-RKF算法得到的经度估计误差为0.027m，最大为0.032m；纬度估计误差均方差为0.005m，最大为0.039m。从数据对比结果可以看出，本发明提出的基于秩滤波的农业机器人组合导航信息融合方法性能优于现有的其它两种交互式多模型信息融合算。

本发明提供了一种基于秩滤波的农业机器人组合导航信息融合方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (7)

1.一种基于秩滤波的农业机器人组合导航信息融合方法，其特征在于，包括如下步骤：

步骤1，根据农业机器人所处环境，将土地平整度和卫星信号强度分级，设定土地平整度因子和卫星信号强度因子ξ，1≤ξ≤ξ_max，ξ∈Z，Z为整数集，ξ_max分别表示最大的土地不平整级别和最大的卫星信号衰落级别；

步骤2，由惯性导航系统INS和全球卫星导航系统GNSS所测量的数据，测量各级别土地平整度和卫星信号衰落情形所对应的量测噪声ν，将量测噪声ν及其协方差矩阵R作为可变参数建立组合导航系统模型集合M，设模型个数为r，各模型之间跳变的Markov转移概率为π_ji，下标i，j分别表示第i个模型和第j个模型；

步骤3，由k-1时刻的模型概率与先验的Markov转移概率π_ji进行交互，计算混合概率k≥1；

步骤4，对于第j个模型，j＝1,2,…，r，利用k-1时刻的状态估计和协方差阵计算k时刻的状态初值与协方差阵初值

步骤5，在组合导航系统获得新的量测z^k之后，利用状态初值与协方差阵初值采用秩滤波算法进行滤波，得到k时刻的状态估计和协方差阵

步骤6，利用混合概率残差估值和残差协方差计算当前时刻的匹配模型对应的残差估值残差协方差似然函数和模型概率

步骤7，利用各个模型滤波器k时刻的状态估计协方差阵和模型概率计算导航参数的全局估计及其估计误差的协方差阵P_k|k；

步骤8，返回步3，重复步骤3至步骤7，直至接收到停止导航控制指令。

2.根据权利要求1所述方法，其特征在于，步骤3中，通过如下公式计算混合概率

${\mathrm{\μ}}_{k-1|k-1}^{i,j}=\frac{{\mathrm{\π}}_{ji}{\mathrm{\μ}}_{k-1}^i}{\underset{j=1}{\overset{r}{\Σ}}{\mathrm{\π}}_{ji}{\mathrm{\μ}}_{k-1}^i}.$

3.根据权利要求2所述方法，其特征在于，步骤4中，通过如下公式计算k时刻的状态初值与协方差阵初值

$\left\{\begin{array}{c}{\hat{x}}_{k-1|k-1}^j=\underset{i=1}{\overset{r}{\Σ}}{\hat{x}}_{k-1|k-1}^i{\mathrm{\μ}}_{k-1|k-1}^{i,j}\\ P_{k-1|k-1}^j=\underset{i=1}{\overset{r}{\Σ}}{\mathrm{\μ}}_{k-1|k-1}^{i,j}\{P_{k-1|k-1}^i+({\hat{x}}_{k-1|k-1}^i-{\hat{x}}_{k-1|k-1}^j){({\hat{x}}_{k-1|k-1}^i-{\hat{x}}_{k-1|k-1}^j)}^T\}\end{array}\right..$

4.根据权利要求3所述方法，其特征在于，步骤5包括如下步骤：

步骤5-1，利用k-1时刻的状态估计一维标准分布概率p_β对应的分位点和第β层采样点的修正系数r_β，通过秩采样机制求取采样点集

$X_{\mathrm{\α},k-1|k-1}^j={\hat{x}}_{k-1|k-1}^j+r_{\mathrm{\β}}{\mathrm{\λ}}_{p_{\mathrm{\β}}}{\left(\sqrt{P_{k-1|k-1}^j}\right)}_l,l=1,2,...n,$

$\left\{\begin{array}{cc}\mathrm{\α}=(\mathrm{\β}-1)n+l,& \mathrm{\β}=1,2,...\mathrm{\ρ}\\ \mathrm{\α}=(\mathrm{\β}-2)n+l,& \mathrm{\β}=\mathrm{\ρ}+2,\mathrm{\ρ}+3,...,2\mathrm{\ρ}+1\end{array}\right.,$

其中，n为状态向量x的维数，ρ为采样点层数，α，β分别表示第α个采样点和第β层采样点，l取值为[1,n]中的整数，表示矩阵平方根的第l列；

步骤5-2，将采样点集进行非线性传播，得到非线性变换后点集

$X_{\mathrm{\α},k|k-1}^{j*}=f(X_{\mathrm{\α},k-1|k-1}^j,\mathrm{\ω}),$

其中，f(·)为状态方程非线性函数，ω为过程噪声向量；

步骤5-3，进一步预测k时刻的状态估计和协方差阵

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}^j=\frac{1}{2\mathrm{\ρ}n}\underset{\mathrm{\α}=1}{\overset{2\mathrm{\ρ}n}{\Σ}}{X}_{\mathrm{\α},k|k-1}^{j*}\\ P_{k|k-1}^j=\frac{1}{\mathrm{\τ}}\underset{\mathrm{\α}=1}{\overset{2\mathrm{\ρ}n}{\Σ}}\{r_{\mathrm{\α}}^{*}(X_{\mathrm{\α},k|k-1}^{j*}-{\hat{x}}_{k|k-1}^j)\×{(X_{\mathrm{\α},k|k-1}^{j*}-{\hat{x}}_{k|k-1}^j)}^T\}+Q\end{array}\right.,$

其中，为第α个采样点对应的协方差修正系数，取值为1，Q为过程噪声方差矩阵，τ为协方差权重系数，通过如下公式计算：

$\mathrm{\τ}=\underset{\mathrm{\β}=1,\mathrm{\β}\≠\mathrm{\ρ}+1}{\overset{2\mathrm{\ρ}+1}{\Σ}}{r}_{\mathrm{\β}}^2{\mathrm{\λ}}_{p_{\mathrm{\β}}}^2;$

步骤5-4，通过秩采样机制求取采样点集

$X_{\mathrm{\α},k|k-1}^j={\hat{x}}_{k|k-1}^j+r_{\mathrm{\β}}{\mathrm{\λ}}_{p_{\mathrm{\β}}}{\left(P_{k|k-1}^j\right)}_l,$

$\left\{\begin{array}{cc}\mathrm{\α}=(\mathrm{\β}-1)n+l,& \mathrm{\β}=1,2,...\mathrm{\ρ}\\ \mathrm{\α}=(\mathrm{\β}-2)n+l,& \mathrm{\β}=\mathrm{\ρ}+2,\mathrm{\ρ}+3,...,2\mathrm{\ρ}+1\end{array}\right.;$

步骤5-5，将采样点集引入观测方程并进行非线性传播，得到预测的量测点集

$Z_{\mathrm{\α},k|k-1}^{j*}=h(X_{\mathrm{\α},k|k-1}^{j*},v^j),$

其中，h(·)为观测方程非线性函数，ν^j为模型j对应的量测噪声向量；

步骤5-6，通过预测的量测点集计算量测估计

${\hat{z}}_{k|k-1}^j=\frac{1}{2\mathrm{\ρ}n}\underset{\mathrm{\β}=1}{\overset{2\mathrm{\ρ}n}{\Σ}}{Z}_{\mathrm{\α},k|k-1}^{j*};$

步骤5-7，通过如下公式计算新息协方差矩阵和互协方差阵

$P_{zz,k|k-1}^j=\frac{1}{\mathrm{\τ}}\underset{\mathrm{\α}=1}{\overset{2\mathrm{\ρ}n}{\Σ}}\{r_{\mathrm{\α}}^{*}(Z_{\mathrm{\α},k|k-1}^{j*}-{\hat{z}}_{k|k-1}^j)\×{(Z_{\mathrm{\α},k|k-1}^{j*}-{\hat{z}}_{k|k-1}^j)}^T\}+R^j,$

$P_{xy,k|k-1}^j=\frac{1}{\mathrm{\τ}}\underset{\mathrm{\α}=1}{\overset{2\mathrm{\ρ}n}{\Σ}}\{r_{\mathrm{\α}}^{*}(X_{\mathrm{\α},k|k-1}^{j*}-{\hat{x}}_{k|k-1}^j)\×{(Z_{\mathrm{\α},k|k-1}^{j*}-{\hat{z}}_{k|k-1}^j)}^T\},$

其中R^j为模型j对应的量测噪声方差矩阵；

步骤5-8，由新息协方差矩阵和互协方差阵计算增益

k时刻组合导航系统的量测值z_k到来之后，通过如下公式计算k时刻的状态估计和协方差阵

$\left\{\begin{array}{c}{\hat{x}}_{k|k}^j={\hat{x}}_{k|k-1}^j+W_k^j(z_k-{\hat{z}}_{k|k-1}^j)\\ P_{k|k}^j=P_{k|k-1}^j-W_k^j{P}_{zz,k|k-1}^j{\left(W_k^j\right)}^T\end{array}\right..$

5.根据权利要求4所述方法，其特征在于，步骤6中，通过如下方法计算当前时刻匹配模型对应的残差估值残差协方差似然函数和模型概率

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_k^j=z^k-{\hat{z}}_{k|k-1}^j\\ S_k^j=E\[\left({\mathrm{\ϵ}}_k^j\right){\left({\mathrm{\ϵ}}_k^j\right)}^T\]=P_{zz,k|k-1}^j\end{array}\right.,$

$\left\{\begin{array}{c}{\mathrm{\Λ}}_k^j={\[{\left(2\mathrm{\π}\right)}^r\left|S_k^j\right|\]}^{-1/2}\exp \{-\frac{1}{2}{\left({\mathrm{\ϵ}}_k^j\right)}^T{\left(S_k^j\right)}^{-1}{\mathrm{\ϵ}}_k^j\}\\ {\mathrm{\μ}}_{k|k-1}^j=\frac{{\mathrm{\Λ}}_k^j\underset{j=1}{\overset{r}{\Σ}}{\mathrm{\π}}_{ji}{\mathrm{\μ}}_{k-1|k-1}^{i,j}}{\underset{i=1}{\overset{r}{\Σ}}{\mathrm{\Λ}}_k^j\underset{j=1}{\overset{r}{\Σ}}{\mathrm{\π}}_{ji}{\mathrm{\μ}}_{k-1|k-1}^{i,j}}\end{array}\right.,$

其中E[·]表示数学期望。

6.根据权利要求5所述方法，其特征在于，步骤7中，通过如下公式计算导航参数的全局估计及其估计误差的协方差阵P_k|k：

$\left\{\begin{array}{c}{\hat{x}}_{k|k}=\underset{j=1}{\overset{r}{\Σ}}{\hat{x}}_{k|k}^j{\mathrm{\μ}}_{k|k-1}^j\\ P_{k|k}=\underset{j=1}{\overset{r}{\Σ}}{\mathrm{\μ}}_{k|k-1}^j\[P_{k|k}^j+({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^j){({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^j)}^T\]\end{array}\right..$

7.根据权利要求6所述的方法，其特征在于，步骤5-1和步骤5-4中，采用中位秩计算第β层采样点对应的一维标准分布概率p_β，计算表达式为p_β＝(β－0.3)/(2ρ+1.4)。