CN108759838A - 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法 - Google Patents

Abstract

本发明公开了基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法，本发明首先根据移动机器人组合导航系统建立SINS/DGPS组合导航系统的状态方程、量测方程及噪声模型；根据非线性贝叶斯估计算法结构，利用秩统计量原理计算采样点和对应的权值，采用时间更新和量测更新过程计算状态估计值与估计误差协方差矩阵。由导航计算机根据组合导航系统的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明能够提高复杂环境下多传感器信息融合系统的滤波精度，使得移动机器人具有适应范围广和抗干扰能力强的优点。

Description

基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法

技术领域

本发明属于机器人导航与控制技术领域，尤其涉及基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法。

背景技术

随着移动机器人性能的不断完善，其应用范围从传统工业应用扩展到农业、医疗、服务业等领域，并在排雷、搜捕、救援、辐射等危险或有害场合具有特殊优势。工作环境的复杂化、非结构化特征对移动机器人的自主性提出了更高的要求。高精度、实时的导航定位是移动机器人实现自主化作业的关键，它需要利用自身携带的各种传感器获取周围的环境信息，通过数据融合处理，提取自身的姿态、位置等参数，从而完成路径规划、导航控制以及后续的各项任务。捷联惯性导航系统(Strapdown inertial navigation system，SINS)能够在短时间内提供高精度、全面的导航参数，移动机器人中多采用以SINS为主，辅以全球导航卫星系统(Global navigation satellite system,GNSS)、视觉传感器等其它传感器的组合导航系统。无论采用何种配置构成组合导航系统，都需要选择一个最优的在线信息融合策略将多种传感器提供的信息进行有效融合，信息融合方法在组合导航系统中起着至关重要的作用。

发明内容

本发明提出了一种基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法，该方法的主要思路是在非线性贝叶斯滤波框架下，利用秩统计量原理计算采样点和对应的权值，采用时间更新和量测更新过程计算状态估计值与估计误差协方差矩阵。秩卡尔曼滤波算法能够提高复杂环境下多传感器信息融合系统的滤波精度，使得移动机器人具有适应范围广和抗干扰能力强的优点。

本发明采用非线性贝叶斯滤波算法框架，利用秩统计量原理计算采样点和对应的权值，采用时间更新和量测更新过程计算状态估计值与估计误差协方差矩阵，本发明具体包括如下步骤：

步骤1，建立导航系统的状态方程、量测方程及噪声模型；

步骤2，根据移动机器人所处环境，设定系统状态初值x₀∈R^m×1、初始协方差矩阵P₀∈R^m×m、初始过程噪声矩阵Q₀∈R^m×m、初始量测噪声矩阵Υ₀∈R^c×c，m、c分别表示系统状态向量和量测向量的维数，R表示实数域空间；

步骤3，设k-1时刻状态向量x的估计值为误差协方差阵为P_k-1|k-1，对误差协方差矩阵P_k-1|k-1进行矩阵分解，得到k-1时刻的左分解矩阵U_k-1|k-1、右分解矩阵V_k-1|k-1和对角矩阵S_k-1|k-1，下标k-1|k-1表示k-1时刻的估计值，即k时刻的初始值；

步骤4，根据k-1时刻的状态估计值采用秩采样机制，获取采样点集{X_i,k-1|k-1}，1≤i≤n，X_i,k-1|k-1表示第i个采样点，n表示采样点个数；

步骤5，通过状态方程对获取的采样点集{X_i,k-1|k-1}进行时间更新，得到一步预测状态以及预测误差协方差阵P_k|k-1，下标k|k-1表示k时刻的一步预测值；

步骤6，对预测误差协方差阵P_k|k-1进行奇异值矩阵分解，得到左分解矩阵U_k|k-1、右分解矩阵V_k|k-1和对角矩阵S_k|k-1；

步骤7，根据一步预测状态采用秩采样机制，获取新的采样点集{X_i,k|k-1}，X_i,k|k-1为新采样点集中的第i个采样点，1≤i≤n，n表示采样点个数；

步骤8，通过量测方程对新的采样点集{X_i,k|k-1}进行非线性传播，得到量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1；

步骤9，利用步骤8中得到的量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1，计算增益矩阵W_k、状态估计和协方差阵估计矩阵P_k|k；

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

步骤1包括：将捷联惯性导航系统(SINS)作为基准公共导航系统，使用SINS的状态量作为系统状态量，使用SINS状态方程作为系统方程；使用差分全球定位系统(DGPS)测量得到的位置和速度信息作为量测量，建立非线性离散系统模型，系统方程具体如下：

其中，公式(1)为状态方程；公式(2)为量测方程；x_k和x_k-1分别为t_k时刻的系统状态向量和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k-1为t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量。

取15维状态向量x，表示为：

其中，δL、δλ、δh分别表示纬度L、经度λ、高度h的变化；δV_E、δV_N、δV_U分别表示东向速度V_E、北向速度V_N、天向速度V_U的变化；φ_E、φ_N、φ_U分别表示纵摇角、横摇角、偏航角；ε_x、ε_y、ε_z分别为陀螺漂移随机常值误差在三个轴上的分量； 分别为加速度计常值误差在三个轴上的分量；[.]^T表示转置矩阵；惯性器件误差方程为：

其中，分别表示东、北、天三个轴上陀螺漂移随机常值误差的变化； 分别表示东、北、天三个轴上加速度计常值误差的变化。

将SINS和DGPS输出的位置差、速度差作为量测值，表示为z：

z＝[L_DGPS-L_SINS,λ_DGPS-λ_SINS,h_DGPS-h_SINS,V_DGPSE-V_SINSE,V_DGPSN-V_SINSN,V_DGPSU-V_SINSU]^T其中，L_DGPS、λ_DGPS、h_DGPS分别表示DGPS测量得到的纬度、经度和高度；L_SINS、λ_SINS、h_SINS分别表示SINS测量得到的纬度、经度和高度；V_DGPSE、V_DGPSN、V_DGPSU分别表示DGPS测量得到的示东向速度、北向速度和天向速度；V_SINSE、V_SINSN、V_SINSU分别表示SINS测量得到的示东向速度、北向速度和天向速度。

步骤3中，通过如下公式对误差协方差矩阵P_k-1|k-1进行奇异值矩阵分解，得到k-1时刻的左分解矩阵U_k-1|k-1、右分解矩阵V_k-1|k-1和对角矩阵S_k-1|k-1：

其中，S_k-1|k-1＝diag(s₁,s₂,…,s_ξ)，s₁≥s₂≥…≥s_ξ≥0，s_ξ为矩阵P_k-1|k-1的第ξ个奇异值；U_k-1|k-1和V_k-1|k-1的列向量分别为矩阵P_k-1|k-1的左奇异向量、右奇异向量，ξ为P_k-1|k-1的秩。

步骤4中，通过如下公式获取采样点集{X_i,k-1|k-1}：

其中，X_i,k-1|k-1为第i个采样点，r_j表示第j层采样点对应的修正系数；λ_pj表示状态估计误差ε所服从分布的一维标准分布概率p_j的分位点；m为k-1时刻状态向量x的维数，ρ为采样点层数，U_l,k-1|k-1表示U_k-1|k-1的第l列，l取值为[1,m]中的整数。

步骤5中，通过如下公式得到一步预测状态以及预测误差协方差阵P_k|k-1：

其中，η_i为第i个采样点对应的协方差修正系数，取值为1，Q_k-1为k-1时刻的过程噪声方差矩阵，τ为协方差权重系数，通过如下公式计算：

步骤6中，通过如下公式进行奇异值矩阵分解，得到左分解矩阵U_k|k-1、右分解矩阵V_k|k-1和对角矩阵S_k|k-1：

步骤7中，通过如下公式计算新的采样点集{X_i,k|k-1}：

步骤8中，通过如下方式对新的采样点集{X_i,k|k-1}进行非线性传播，并计算量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1：

其中，Z_i,k|k-1为新采样点X_i,k|k-1经过非线性传播得到的量测样点；h(·)为量测方程非线性函数；ν_k、γ_k分别为k时刻的量测噪声向量以及对应的方差矩阵，r_i ^*为第i个采样点对应的协方差修正系数，一般取r_i ^*＝1。

步骤9中，通过如下公式计算增益矩阵W_k、状态估计和协方差矩阵估计P_k|k：

其中，z_k为传感器在k时刻得到的量测值。

本发明针对现有技术的不足，采用非线性贝叶斯滤波算法框架建立了秩卡尔曼滤波算法，解决非线性、非高斯系统中的状态估计问题，提高了移动机器人多传感器信息融合算法性能。本发明内容同样适用于组合导航、目标跟踪与识别、机器视觉等其他多传感器信息融合及多源数据处理应用领域。

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

(1)本发明采用非线性贝叶斯滤波理论框架，利用秩统计量原理计算采样点和对应的权值，建立的秩卡尔曼滤波算法不受高斯分布条件限制，相比于传统的高斯滤波算法，能够提高复杂环境下多传感器信息融合系统的滤波精度。

(2)本发明采用时间更新和量测更新两个过程，与传统的高斯滤波算法不同，将估计得到的误差协方差矩阵经过奇异值矩阵分解，利用分解矩阵计算时间更新和量测更新过程中需要的采样点集，该方法能够有效避免数值计算误差积累，提升多传感器信息融合算法的稳定性。

附图说明

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

图1秩卡尔曼滤波算法结构图；

图2移动机器人仿真运行轨迹；

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

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

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

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

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

图8纬度估计误差曲线；

图9经度估计误差曲线。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

本发明的基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法，根据非线性贝叶斯估计算法结构，利用秩统计量原理计算采样点和对应的权值，采用时间更新和量测更新过程计算状态估计值与估计误差协方差矩阵，如图1所示为秩卡尔曼滤波算法结构图。现以捷联惯性导航系统(Strapdown inertial navigation system，SINS)和差分全球定位系统(Differential Global Positioning System,DGPS)构成的移动机器人多传感器信息融合系统为例来说明所提供的状态估计方法，具体步骤如下：

步骤1，建立SINS/DGPS组合导航系统的状态方程、量测方程及噪声模型，以东北天(ENU)地理坐标系为导航系，SINS/DGPS组合导航的系统方程为：

其中，公式(1)为状态方程；公式(2)为量测方程；x_k和x_k-1分别为t_k时刻的系统状态向量和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k-1为t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量。

取15维状态向量x，表示为：

其中，δL、δλ、δh分别表示纬度L、经度λ、高度h的变化；δV_E、δV_N、δV_U分别表示东向速度V_E、北向速度V_N、天向速度V_U的变化；φ_E、φ_N、φ_U分别表示纵摇角、横摇角、偏航角；ε_x、ε_y、ε_z分别为陀螺漂移随机常值误差在三个轴上的分量； 分别为加速度计常值误差在三个轴上的分量；[.]^T表示转置矩阵；惯性器件误差方程为：

其中，分别表示东、北、天三个轴上陀螺漂移随机常值误差的变化； 分别表示东、北、天三个轴上加速度计常值误差的变化。

将SINS和DGPS输出的位置差、速度差作为量测值，表示为：

z＝[L_DGPS-L_SINS,λ_DGPS-λ_SINS,h_DGPS-h_SINS,V_DGPSE-V_SINSE,V_DGPSN-V_SINSN,V_DGPSU-V_SINSU]^T其中，L_DGPS、λ_DGPS、h_DGPS分别表示DGPS测量得到的纬度、经度和高度；L_SINS、λ_SINS、h_SINS分别表示SINS测量得到的纬度、经度和高度；V_DGPSE、V_DGPSN、V_DGPSU分别表示DGPS测量得到的示东向速度、北向速度和天向速度；V_SINSE、V_SINSN、V_SINSU分别表示SINS测量得到的示东向速度、北向速度和天向速度。

步骤2，根据移动机器人所处环境，设定系统状态初值x₀为15维零向量，初始协方差矩阵P₀、初始过程噪声矩阵Q₀和初始量测噪声矩阵R₀为：

P₀＝diag{(0m)²,(0m)²,(0m)²，(0.05m/s)²,(0.05m/s)²,(0.05m/s)²,(1°)²,(1°)²,(1°)²,

(0.1°/h)²,(0.1°/h)²,(0.1°/h)²,(0.0001g)²,(0.0001g)²,(0.0001g)²}

Q₀＝diag{(0.0001g)²,(0.0001g)²,(0.0001g)²,(0.03°/h)²,(0.03°/h)²,(0.03°/h)²,0,0,0,0,0,0,0,0,0}

R₀＝diag{(0.5m)²,(0.5m)²,(0.5m)²,(0.05m/s)²,(0.05m/s)²,(0.05m/s)²}

其中，m、m/s、°、°/h分别为单位米、米/秒，度、度/小时；g为重力加速度，取值为9.8m/s²。

步骤3，设k-1时刻状态向量x的估计值为误差协方差阵为P_k-1|k-1，对误差协方差矩阵P_k-1|k-1进行矩阵分解，得到k-1时刻的左分解矩阵U_k-1|k-1、右分解矩阵V_k-1|k-1和对角矩阵S_k-1|k-1：

其中，S_k-1|k-1＝diag(s₁,s₂,…,s_ξ)，s₁≥s₂≥…≥s_ξ≥0，s_ξ为矩阵P_k-1|k-1的第ξ个奇异值；U_k-1|k-1和V_k-1|k-1的列向量分别为矩阵P_k-1|k-1的左奇异向量、右奇异向量，ξ为P_k-1|k-1的秩。

步骤4，根据k-1时刻的状态估计采用秩采样机制，获取采样点集{X_i,k-1|k-1}，1≤i≤n：

其中，X_i,k-1|k-1为第i个采样点，r_j表示第j层采样点对应的修正系数；表示状态估计误差ε所服从分布的一维标准分布概率p_j的分位点；m为k-1时刻状态向量x的维数，ρ为采样点层数，U_l,k-1|k-1表示U_k-1|k-1的第l列，l取值为[1,m]中的整数。

步骤5，时间更新过程，通过状态方程对获取的采样点集{X_i,k-1|k-1}进行时间更新，得到一步预测状态以及预测误差协方差阵P_k|k-1：

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

步骤6，对预测误差协方差阵P_k|k-1进行奇异值矩阵分解，得到左分解矩阵U_k|k-1、右分解矩阵V_k|k-1和对角矩阵S_k|k-1：

步骤7，根据一步预测状态采用秩采样机制，获取新的采样点集{X_i,k|k-1}：

步骤8，量测更新过程，通过量测方程对获取的采样点集{X_i,k|k-1}进行非线性传播，得到量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1：

其中，Z_i,k|k-1为新采样点X_i,k|k-1经过非线性传播得到的量测样点；h(·)为量测方程非线性函数；ν_k、Υ_k分别为k时刻的量测噪声向量以及对应的方差矩阵，r_i ^*为第i个采样点对应的协方差修正系数，一般取r_i ^*＝1。

步骤9，利用步骤8中得到的量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1，计算增益矩阵W_k、状态估计和协方差阵估计P_k|k：

式中，z_k为传感器在k时刻得到的量测值。

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

实施例

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

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

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

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

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

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

DGPS位置测量精度为0.1米；

SINS捷联惯性导航系统采样周期为10ms，DGPS采样周期为1s。

将状态初值x₀设为零矩阵0_15×1，初始协方差矩阵P₀、初始过程噪声矩阵Q₀和初始量测噪声矩阵R₀分别设置为：

P₀＝diag{(0m)²,(0m)²,(0m)²，(0.05m/s)²,(0.05m/s)²,(0.05m/s)²,(1°)²,(1°)²,(1°)²,

(0.1°/h)²,(0.1°/h)²,(0.1°/h)²,(0.0001g)²,(0.0001g)²,(0.0001g)²}

Q₀＝diag{(0.0001g)²,(0.0001g)²,(0.0001g)²,(0.03°/h)²,(0.03°/h)²,(0.03°/h)²,0,0,0,0,0,0,0,0,0}

R₀＝diag{(0.5m)²,(0.5m)²,(0.5m)²,(0.05m/s)²,(0.05m/s)²,(0.05m/s)²}

其中，m、m/s、°、°/h分别为单位米、米/秒，度、度/小时；g为重力加速度，取值为9.8m/s²。

仿真时间为3600s，假设移动机器人在某片区域中依照“割草机”模式行进，其运动轨迹如图2所示。在同等条件下，导航计算机根据传感器数据和系统模型，分别采用扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)和本发明提出的秩卡尔曼滤波(RKF)算法进行滤波估计，对比三种算法在组合导航系统中的估计结果。图3为俯仰角估计误差曲线，图4为横摇角估计误差曲线图，图5为航向角估计误差曲线，可以看出，三种算法所得到的水平姿态角误差均能收敛到很小的值并且收敛速度较快，航向角误差的修正效果不是很明显，但也能看出逐渐收敛的趋势，收敛速度较慢。从航向角误差的收敛过程可以看出，本发明提出的RKF算法估计效果优于其它两种算法；图6、图7分别为东向、北向速度估计误差曲线，由于DGPS提供的速度信息对SINS的速度误差进行修正，采用RKF算法速度误差基本保持在0.2m/s以内，明显优于其它两种算法；图8、图9分别为纬度、经度估计误差曲线，可以看出UKF与RKF算法位置误差较为接近，EKF算法误差较大。进一步的，将三种算法得到的位置估计误差进行数值比较，采用EKF算法得到的纬度估计误差均值为1.54m，最大达到2.69m；经度估计误差均值为1.56m，最大达到2.93m。采用UKF算法得到的纬度估计误差均值为0.84m，最大为1.27m；经度估计误差均值为0.68m，最大为1.13m。采用RKF算法得到的纬度估计误差均值为0.47m，最大为0.64m；经度估计误差均值为0.38m，最大为0.57m。从仿真结果能够看出，RKF算法估计精度优于EKF和UKF算法。从数据对比结果可以看出，本发明提出的基于秩卡尔曼滤波的移动机器人多传感器信息融合方法性能优于现有的其它两种高斯滤波信息融合算法。

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

Claims (9)

1.基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法，其特征在于，包括如下步骤：

步骤1，建立导航系统的状态方程、量测方程及噪声模型；

步骤2，根据移动机器人所处环境，设定系统状态初值x₀∈R^m×1、初始协方差矩阵P₀∈R^{m ×m}、初始过程噪声矩阵Q₀∈R^m×m、初始量测噪声矩阵Υ₀∈R^c×c，m、c分别表示系统状态向量的维数和量测向量的维数，R表示实数域空间；

步骤3，设k-1时刻状态向量x的估计值为误差协方差阵为P_k-1|k-1，对误差协方差矩阵P_k-1|k-1进行矩阵分解，得到k-1时刻的左分解矩阵U_k-1|k-1、右分解矩阵V_k-1|k-1和对角矩阵S_k-1|k-1，下标k-1|k-1表示k-1时刻的估计值，即k时刻的初始值；

步骤4，根据k-1时刻的状态估计值采用秩采样机制，获取采样点集{X_i,k-1|k-1}，1≤i≤n，X_i,k-1|k-1表示第i个采样点，n表示采样点个数；

步骤5，通过状态方程对获取的采样点集{X_i,k-1|k-1}进行时间更新，得到一步预测状态以及预测误差协方差阵P_k|k-1，下标k|k-1表示k时刻的一步预测值；

步骤6，对预测误差协方差阵P_k|k-1进行奇异值矩阵分解，得到左分解矩阵U_k|k-1、右分解矩阵V_k|k-1和对角矩阵S_k|k-1；

步骤7，根据一步预测状态采用秩采样机制，获取新的采样点集{X_i,k|k-1}，X_i,k|k-1为新采样点集中的第i个采样点，1≤i≤n，n表示采样点个数；

步骤8，通过量测方程对新的采样点集{X_i,k|k-1}进行非线性传播，得到量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xy,k|k-1；

步骤9，利用步骤8中得到的量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1，计算增益矩阵W_k、状态估计和协方差阵估计矩阵P_k|k；

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

2.根据权利要求1所述的方法，其特征在于，步骤1包括：将捷联惯性导航系统SINS作为基准公共导航系统，使用SINS的状态量作为系统状态量，使用SINS状态方程作为系统方程；使用差分全球定位系统DGPS测量得到的位置和速度信息作为量测量，建立非线性离散系统模型，系统方程具体如下：

其中，公式(1)为状态方程；公式(2)为量测方程；x_k和x_k-1分别为t_k时刻的系统状态向量和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k-1为t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量；

取15维状态向量x，表示为：

x＝[δL,δλ,δh,δV_E,δV_N,δV_U,φ_E,φ_N,φ_U,ε_x,ε_y,ε_z,▽_x,▽_y,▽_z]^T，

其中，δL、δλ、δh分别表示纬度L、经度λ、高度h的变化；δV_E、δV_N、δV_U分别表示东向速度V_E、北向速度V_N、天向速度V_U的变化；φ_E、φ_N、φ_U分别表示纵摇角、横摇角、偏航角；ε_x、ε_y、ε_z分别为陀螺漂移随机常值误差在三个轴上的分量；

▽_x、▽_y、▽_z分别为加速度计常值误差在三个轴上的分量；[.]^T表示转置矩阵；

惯性器件误差方程为：

其中，分别表示东、北、天三个轴上陀螺漂移随机常值误差的变化； 分别表示东、北、天三个轴上加速度计常值误差的变化；

将SINS和DGPS输出的位置差、速度差作为量测值，表示为z：

z＝[L_DGPS-L_SINS,λ_DGPS-λ_SINS,h_DGPS-h_SINS,V_DGPSE-V_SINSE,V_DGPSN-V_SINSN,V_DGPSU-V_SINSU]^T

其中，L_DGPS、λ_DGPS、h_DGPS分别表示DGPS测量得到的纬度、经度和高度；L_SINS、λ_SINS、h_SINS分别表示SINS测量得到的纬度、经度和高度；V_DGPSE、V_DGPSN、V_DGPSU分别表示DGPS测量得到的示东向速度、北向速度和天向速度；V_SINSE、V_SINSN、V_SINSU分别表示SINS测量得到的示东向速度、北向速度和天向速度。

3.根据权利要求2所述的方法，其特征在于，步骤3中，通过如下公式对误差协方差矩阵P_k-1|k-1进行奇异值矩阵分解，得到k-1时刻的左分解矩阵U_k-1|k-1、右分解矩阵V_k-1|k-1和对角矩阵S_k-1|k-1：

其中，S_k-1|k-1＝diag(s₁,s₂,…,s_ξ)，s₁≥s₂≥…≥s_ξ≥0，s_ξ为矩阵P_k-1|k-1的第ξ个奇异值；U_k-1|k-1和V_k-1|k-1的列向量分别为矩阵P_k-1|k-1的左奇异向量、右奇异向量，ξ为P_k-1|k-1的秩。

4.根据权利要求3所述的方法，其特征在于，步骤4中，通过如下公式获取采样点集{X_i,k-1|k-1}：

其中，X_i,k-1|k-1为第i个采样点，r_j表示第j层采样点对应的修正系数；表示状态估计误差ε所服从分布的一维标准分布概率p_j的分位点；m为k-1时刻状态向量x的维数，ρ为采样点层数，U_l,k-1|k-1表示U_k-1|k-1的第l列，l取值为[1,m]中的整数。

5.根据权利要求4所述的方法，其特征在于，步骤5中，通过如下公式得到一步预测状态以及预测误差协方差阵P_k|k-1：

其中，η_i为第i个采样点对应的协方差修正系数，取值为1，Q_k-1为k-1时刻的过程噪声方差矩阵，τ为协方差权重系数，通过如下公式计算：

6.根据权利要求5所述的方法，其特征在于，步骤6中，通过如下公式进行奇异值矩阵分解，得到左分解矩阵U_k|k-1、右分解矩阵V_k|k-1和对角矩阵S_k|k-1：

7.根据权利要求6所述的方法，其特征在于，步骤7中，通过如下公式计算新的采样点集{X_i,k|k-1}：

8.根据权利要求7所述的方法，其特征在于，步骤8中，通过如下方式对新的采样点集{X_i,k|k-1}进行非线性传播，并计算量测预测值估计量测新息协方差矩阵估计P_zz,k|k-1以及量测互协方差矩阵估计P_xz,k|k-1：

其中，Z_i,k|k-1为新采样点X_i,k|k-1经过非线性传播得到的量测样点；h(·)为量测方程非线性函数；ν_k、Υ_k分别为k时刻的量测噪声向量以及对应的方差矩阵，r_i ^*为第i个采样点对应的协方差修正系数。

9.根据权利要求8所述的方法，其特征在于，步骤9中，通过如下公式计算增益矩阵W_k、状态估计和协方差矩阵估计P_k|k：

其中，z_k为传感器在k时刻得到的量测值。