强动态条件下的卫星编队相对位置鲁棒高精度估计方法

本发明属于卫星编队控制技术领域，涉及强动态条件下的卫星编队相对位置鲁棒高精度估计方法。

背景技术：

卫星编队技术在当前与未来的空间技术发展与应用中起着重要作用，编队间的相对导航技术是支撑卫星编队的关键技术之一，而编队卫星受控系统的特性使得其具有模型时变性较强的特点，该特性会使得滤波算法的精度下降。当前在卫星编队的相对导航技术中应用最为成熟、广泛地是采用扩展卡尔曼滤波算法ekf，其特点是成熟稳定、可靠性强，已经在登月工程等大型航天工程中得到使用，但其弊端则在于近似线性化的手段在处理一些非线性较强的系统时精度下降。

技术实现要素：

为了达到上述目的，本发明提供强动态条件下的卫星编队相对位置鲁棒高精度估计方法，采用相对动力学模型，抵消卫星编队相对运动模型的舍入误差，从而实现长时间、远距离、受控系统下的高精度鲁棒相对位置估计，并且在系统模型不确定性较强、外部扰动较大时系统精度较高，解决了现有技术中卫星编队相对运动模型带有舍入误差且滤波算法在系统模型不确定性较强、外部扰动较大时系统精度下降的问题。

本发明所采用的技术方案是，强动态条件下的卫星编队相对位置鲁棒高精度估计方法，包括以下步骤：

步骤s10、根据建立的编队卫星受控系统的状态矩阵，作为主-从模式的卫星编队的相对动力学模型；

步骤s20：对建立的编队卫星受控系统的状态矩阵进行离散化处理，建立编队卫星受控系统的状态矩阵的离散化形式；

步骤s31、鲁波滤波算法的初值更新；

步骤s32、鲁波滤波算法的滤波过程，包括如下计算步骤：

步骤s32-1，一步预测：计算当前时刻k的编队卫星受控系统的一步状态预测矩阵与一步状态预测矩阵的误差方差矩阵p(k|k-1)；

步骤s32-2、残差计算：读取当前时刻k的编队卫星受控系统的外部直接量测向量y(k)，计算当前时刻k的量测残差

步骤s32-3、鲁棒切换：

步骤s32-3.1、计算误差方差判别矩阵

步骤s32-3.2、计算调节矩阵l(k)；

步骤s32-3.3、进行鲁棒切换并给出滤波增益矩阵σ(k)，如下式所示

式中，p^-1(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵的逆矩阵，p(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵，p(k)表示当前时刻k的状态估计矩阵的误差方差矩阵，α为待定正常数；γ表示渐消因子；表示误差方差判别矩阵；

步骤s33、新息：对误差方差矩阵、滤波增益矩阵σ(k)、状态估计更新进行更新；

步骤s34、算法迭代：

在得到第k个时刻的状态估计矩阵的误差方差矩阵p(k)与状态估计之后，返回步骤s10，重新获取轨道参数计算相关矩阵，并进入鲁棒滤波算法，直到给出指令，鲁棒滤波算法结束；

步骤s35、误差约束与参数选取：

鲁棒性渐消因子γ满足下式：

{maxeig(p(k))}^1/2＜γ＜{maxeig(p(k)-σ(k))}^-1/2；

ε²i≤σ^-1(k)

式中，算子maxeig(a)为矩阵a的最大特征值；ε表示主星轨道角加速度；i为单位矩阵；σ^-1(k)表示滤波增益矩阵的逆矩阵；

系统状态估计误差满足下式：

式中，μ为正的常数且0＜λ＜1，k为当前时刻，表示系统状态估计误差初值，e{δ}表示变量δ期望，pmin、pmax为正常数且满足pmaxi≥p(k)≥pmini。

进一步地，步骤s10中，根据建立的编队卫星受控系统的状态矩阵，作为主-从模式的卫星编队的相对动力学模型，具体为：

首先，对于主-从模式的卫星编队，建立主星轨道坐标系：主星轨道坐标系包括相互垂直的x轴、y轴、z轴，x轴由主星质心指向地球质心，y轴位于轨道平面内垂直于x轴且与速度方向夹角为锐角，z轴与x、y轴构成右手系，定义从星质心到主星质心的向量为ρ，则ρ在主星轨道坐标系下的三轴投影分量分别为x、y、z，同时三轴投影x、y、z关于时间的一阶导数，即从星质心相对于主星质心的速度在三轴的投影分别为

其次，在主星轨道坐标系下，建立编队卫星受控系统的状态矩阵作为主-从模式的卫星编队的相对动力学模型，如下式所示：

式中，a表示编队卫星受控系统的系统参数矩阵，x表示编队中从星相对于主星的广义状态变量，u表示量测误差矩阵；

式中，编队中从星相对于主星的广义状态变量x，如下式所示：

式中，算子[·]^t表示矩阵的转置；

编队卫星受控系统的系统参数矩阵a，如下式所示：

式中，ω为主星轨道角速度，μ表示引力常量，r1表示从星质心到地球质心的向量，ε表示主星轨道角加速度，||r1||表示r1的2-范数；

量测误差矩阵u，如下式所示：

式中，atx表示从星加速度at在主星轨道坐标系下的x轴投影，aty表示从星加速度at在主星轨道坐标系下的y轴投影，atz分别表示从星加速度at在主星轨道坐标系下的z轴投影，r2表示主星质心到地球质心的向量，||r2||为r2的2-范数。

进一步地，步骤s20中，编队卫星受控系统的状态矩阵的离散化形式，如下式所示：

x(k+1)＝φx(k)+γ

φ＝e^aδt,γ＝uδt；

式中，x(k+1)表示下一个待估计时刻的编队卫星受控系统的状态矩阵，x(k)表示当前时刻编队卫星受控系统的状态矩阵，φ为编队卫星受控系统的状态转移矩阵，γ表示当前时刻编队卫星受控系统的过程噪声矩阵，e^aδt表示矩阵级数，δt表示滤波步长，u表示量测误差矩阵，a表示编队卫星受控系统的系统参数矩阵；

矩阵级数e^aδt的计算，如下式所示：

式中，e作为数学常数，是自然对数函数的底数；k表示当前时刻；k！表示k的阶乘；i表示某一时刻；i！表示i的阶乘；i6为6阶单位矩阵。

进一步地，步骤s31中，鲁波滤波算法的初值更新，具体为：

若当前时刻k的鲁棒滤波算法处于第一步亦即k＝1，则对于当前时刻k＝1的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k)k＝1进行手动赋值；

若当前时刻k的鲁棒滤波算法非第一步亦即k＞1，则对于当前时刻k的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵的初值为上一时刻k-1时刻通过鲁棒滤波算法得到的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k-1)。

进一步地，步骤s32-1，计算当前时刻k的编队卫星受控系统的一步状态预测矩阵与一步状态预测矩阵的误差方差矩阵p(k|k-1)，计算公式分别如下式所示：

p(k|k-1)＝φ(k)p(k-1)φ^t(k)+w(k)；

式中，表示当前时刻k的编队卫星受控系统的一步状态预测矩阵，φ(k)表示当前时刻k的状态转移矩阵，为上一时刻k-1的编队卫星受控系统的状态估计矩阵，γ表示当前时刻k编队卫星受控系统的过程噪声，p(k|k-1)表示当前时刻k的一步状态预测矩阵的误差方差矩阵，p(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵，φ(k)^t表示当前时刻k的状态转移矩阵φ(k)的转置矩阵，w(k)为当前时刻k的编队卫星受控系统的过程噪声方差矩阵。

进一步地，步骤s32-2中，当前时刻k的量测残差的计算如下式所示：

式中，y(k)表示当前时刻k的编队卫星受控系统的外部直接量测向量，h表示编队卫星受控系统的状态矩阵x到测量向量y的转移矩阵，表示当前时刻k的编队卫星受控系统的一步状态预测矩阵。

进一步地，步骤s32-3.1中，误差方差判别矩阵的计算如下式所示：

式中，表示当前时刻k的量测残差的转置矩阵；λ为遗忘因子。

进一步地，步骤s32-3.2中，调节矩阵l(k)的计算如下式所示：

l(k)＝γ(p^-1(k|k-1)-κ²i6)^1/2；

式中，γ表示渐消因子，为正的常数；p^-1(k|k-1)表示当前时刻的一步状态预测矩阵的误差方差矩阵的逆矩阵；κ表示常数，i6为6阶单位矩阵。

进一步地，步骤s33中，对误差方差矩阵、滤波增益矩阵σ(k)、状态估计更新进行更新，包括以下步骤：

误差方差矩阵更新：

p(k)＝hσ(k)h^t+v(k)；

式中，σ(k)表示增益调度矩阵，h^t表示编队卫星受控系统的状态矩阵x到测量向量y的转移矩阵的逆矩阵；v(k)为系统量测误差方差矩阵；

滤波增益矩阵σ(k)更新：

k(k)＝σ(k)h^tp^-1(k)；

式中，k(k)表示滤波增益矩阵的更新矩阵；p^-1(k)表示当前时刻k的状态估计矩阵的误差方差矩阵p(k)的逆矩阵；

状态估计更新：

式中，表示当前时刻k的编队卫星受控系统的一步状态预测矩阵。

本发明的有益效果是：本发明强动态条件下的卫星编队相对位置鲁棒高精度估计方法，在系统模型不确定性较强、外部扰动较大时系统精度较高的条件下，考虑到经典的扩展卡尔曼滤波将模型近似线性化的方法在处理本发明所针对的受控卫星编队系统时会引入较大的模型舍入误差，本发明所提出的鲁棒滤波算法将各类误差视为扰动，通过建立误差估计约束准则约束滤波参数与估计误差之间的关系，避免模型线性化带来的误差无法抑制的弊端，并引入鲁棒切换方法实现系统在强鲁棒性与最优估计之间的切换，从而相比较于经典卡尔曼滤波算法实现精度提升。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是经典ekf的相对位置误差曲线。

图2是经典ekf的相对速度误差曲线。

图3是本发明鲁棒滤波算法的相对位置误差曲线。

图4是本发明鲁棒滤波算法的相对速度误差曲线。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

针对上述特点，本发明采用鲁棒滤波算法处理编队卫星受控系统的强时变特性，实现长时间、远距离、受控系统下的高精度鲁棒相对位置估计。

本发明强动态条件下的卫星编队相对位置鲁棒高精度估计方法，具体包括如下步骤：

步骤s10、根据建立的编队卫星受控系统的状态矩阵，作为主-从模式的卫星编队的相对动力学模型，具体为：

首先，对于主-从模式的卫星编队(两颗及两颗以上)，建立主星轨道坐标系(坐标系原点与主星质心)：主星轨道坐标系包括相互垂直的x轴、y轴、z轴，x轴由主星质心指向地球质心，y轴位于轨道平面内垂直于x轴且与速度方向夹角为锐角，z轴与x、y轴构成右手系，下角标1表示从星状态，下角标2表示主星状态，定义从星质心到主星质心的向量为ρ(亦即在主星轨道坐标系下从星质心坐标向量)，则ρ在主星轨道坐标系下的三轴投影分量分别为x、y、z，同时三轴投影x、y、z关于时间的一阶导数，即从星质心相对于主星质心的速度在三轴的投影分别为

其次，在主星轨道坐标系下，建立编队卫星受控系统的状态矩阵作为主-从模式的卫星编队的相对动力学模型，如式(1)所示：

式(1)中，a表示编队卫星受控系统的系统参数矩阵，x表示编队中从星相对于主星的广义状态变量，u表示量测误差矩阵。

式(1)中，编队中从星相对于主星的广义状态变量x，如式(2)所示：

式(2)中，算子[·]^t表示矩阵的转置。

式(1)中，编队卫星受控系统的系统参数矩阵a，如式(3)所示：

式(3)中，ω为主星轨道角速度，μ表示引力常量，r1表示从星质心到地球质心的向量，ε表示主星轨道角加速度，||r1||表示r1的2-范数。

式(1)中，量测误差矩阵u，如式(4)所示：

式(4)中，atx表示从星加速度at在主星轨道坐标系下的x轴投影，aty表示从星加速度at在主星轨道坐标系下的y轴投影，atz分别表示从星加速度at在主星轨道坐标系下的z轴投影，r2表示主星质心到地球质心的向量，||r2||为r2的2-范数。

步骤s20：对步骤s10建立的编队卫星受控系统的状态矩阵(式(1))进行离散化处理，建立编队卫星受控系统的状态矩阵的离散化形式，如式(5)所示，用于适应本申请鲁棒滤波算法，使得采用本申请的鲁棒滤波算法在系统模型不确定性较强、外部扰动较大时系统精度较高：

x(k+1)＝φx(k)+γ

φ＝e^aδt,γ＝uδt(5)；

式(5)中，x(k+1)表示下一个待估计时刻的编队卫星受控系统的状态矩阵，x(k)表示当前时刻编队卫星受控系统的状态矩阵，φ为编队卫星受控系统的状态转移矩阵，γ表示当前时刻编队卫星受控系统的过程噪声矩阵，e^aδt表示矩阵级数，δt表示滤波步长。

式(5)中，添加γ矩阵的目的在于使编队卫星受控系统的状态矩阵添加非线性项，抵消卫星编队相对动力学模型的舍入误差。

式(5)中，矩阵级数e^aδt的计算，如式(6)所示：

式(6)中，e作为数学常数，是自然对数函数的底数，k表示当前时刻；k！表示k的阶乘；i表示某一时刻；i！表示i的阶乘；i6为6阶单位矩阵，在具体运算时一般只需要取前四阶级数即可，所取阶次越高计算精度也越高，但运算量也会随之增大。

步骤s30、鲁棒滤波算法的滤波流程：

步骤s31、鲁波滤波算法的初值更新：

若当前时刻k的鲁棒滤波算法处于第一步亦即k＝1，则对于当前时刻k＝1的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k)k＝1进行手动赋值，编队卫星受控系统的状态估计矩阵的初值可以通过经验得到，或者将其置为零向量，状态估计矩阵的误差方差矩阵p(k)k＝1的初值可以通过经验进行赋值，或者将其置为简单的单位矩阵；

若当前时刻k的鲁棒滤波算法非第一步亦即k＞1，则对于当前时刻k的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵的初值为上一时刻k-1时刻通过鲁棒滤波算法得到的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k-1)。

步骤s32、鲁波滤波算法的滤波过程：若当前时刻k的鲁棒滤波算法非第一步亦即k＞1，根据上一时刻k-1得到的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k-1)，经过鲁棒滤波算法得到当前时刻k的误差方差矩阵p(k)与状态估计矩阵其具体计算步骤如下：

步骤s32-1，一步预测：

对于当前时刻k，根据上一时刻k-1得到的编队卫星受控系统的状态估计矩阵与状态估计矩阵的误差方差矩阵p(k-1)对当前时刻k的编队卫星受控系统的一步状态预测矩阵与一步状态预测矩阵的误差方差矩阵p(k|k-1)进行计算，计算公式分别如式(7)和式(8)所示：

p(k|k-1)＝φ(k)p(k-1)φ^t(k)+w(k)(8)；

式中，表示当前时刻k的编队卫星受控系统的一步状态预测矩阵，φ(k)表示当前时刻k的状态转移矩阵，为上一时刻k-1的编队卫星受控系统的状态估计矩阵，γ表示当前时刻k编队卫星受控系统的过程噪声，p(k|k-1)表示当前时刻k的一步状态预测矩阵的误差方差矩阵，p(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵，φ(k)^t表示当前时刻k的状态转移矩阵φ(k)的转置矩阵，w(k)为当前时刻k的编队卫星受控系统的过程噪声方差矩阵。

通过编队卫星受控系统的一步状态预测矩阵(式7)、一步状态预测矩阵的误差方差矩阵(式8)对编队卫星受控系统进行初步状态估计，式(7)、式(8)作为后续得到编队卫星受控系统鲁棒切换判据与最终系统状态估计的基础。

步骤s32-2、残差计算：

读取当前时刻k的编队卫星受控系统的外部直接量测向量y(k)，基于编队卫星受控系统的状态矩阵x到测量向量y的转移矩阵h与当前时刻k的编队卫星受控系统的一步状态预测矩阵计算当前时刻k的量测残差

式中，y(k)表示当前时刻k的编队卫星受控系统的外部直接量测向量，h表示编队卫星受控系统的状态矩阵x到测量向量y的转移矩阵，表示当前时刻k的编队卫星受控系统的一步状态预测矩阵。

步骤s32-3、鲁棒切换，使得鲁棒滤波算法能够合理控制在最优和鲁棒两个工作状态之间：其中，系统首先通过上一时刻k-1的状态估计矩阵的误差方差矩阵p(k-1)以及k的量测残差得到误差方差判别矩阵通过判别误差方差判别矩阵与上一时刻k-1的状态估计矩阵的误差方差矩阵p(k-1)的大小关系判断经典卡尔曼滤波方法的误差方法递推方法是否失效，若未失效，则转入经典ekf误差方差递推方法，若失效则转入鲁棒滤波递推方法。

步骤s32-3.1、首先计算误差方差判别矩阵该矩阵用于之后的步骤中判断经典扩展卡尔曼滤波算法的误差方差递推方法是否失效：

式(10)中，λ为遗忘因子，一般选取该值为正，同时该值越大滤波算法越逼近于传统意义上的ekf(扩展卡尔曼滤波)，亦即系统恢复最优估计准则但鲁棒性下降，反之系统在牺牲一定精度条件下能够获得较为优良的鲁棒性。一般而言，λ∈(0,1)，其值越小系统鲁棒性越强，其值越大(10)中修正效果越弱，系统越靠近于经典ekf。

步骤s32-3.2、随后基于误差方差矩阵的一步预测p(k|k-1)计算调节矩阵l(k)，该矩阵选取方法较多，经典选取方法有对角正定矩阵、误差方差矩阵平方等：

l(k)＝γ(p^-1(k|k-1)-κ²i6)^1/2(11)

式(11)中，γ表示渐消因子，为正的常数；p^-1(k|k-1)表示当前时刻的一步状态预测矩阵的误差方差矩阵的逆矩阵；κ表示一个较小的常数，其值可以通过仿真、实验、经验等方法针对具体工况确定最优值，本发明中κ选取为0.05，通常其值越大系统越逼近于经典ekf，i6为6阶单位矩阵。

步骤s32-3.3、最后进行鲁棒切换并给出增益调度矩阵σ(k)，该矩阵作用是在经典ekf误差方差递推仍然成立时(亦即式(12)中满足判别)维持增益调度矩阵σ(k)为当前时刻k的状态估计矩阵的误差方差矩阵p(k)不变，而在其失效时(亦即式(12)中满足判别)通过调节增益调度矩阵σ(k)为σ(k)＝(p^-1(k-1)-γ²l^t(k)l(k))^-1，用来调整误差方差矩阵：

式(12)中，p^-1(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵的逆矩阵，p(k-1)表示上一时刻k-1的状态估计矩阵的误差方差矩阵，p(k)表示当前时刻k的状态估计矩阵的误差方差矩阵，α为待定正常数，一般而言α可由经验或者仿真调试得到，本发明中α选取0.8。

对于典型的线性h∞鲁棒滤波算法，其设计指标是保证在可能的最大线性化误差下滤波的稳定性，其增益调度算子σ常取为(12)中的第一式，渐消因子γ定义为估计误差幅值与其它所有误差强度之和比值的上确界，这些误差包括量测噪声、外部扰动(自然摄动和人为机动)，以及非线性模型误差(或线性化误差)，这个上确界保证了鲁棒性和最优性能够到达所设定的平衡，随着γ的提升，滤波算法的鲁棒性能降低，而当γ→∞时，鲁棒滤波则退化为标准的卡尔曼滤波。但传统的鲁棒滤波算法常将渐消因子视为自定义变量，视环境人为定义，这显然存在极大的弊端，因为一旦γ给定，算法将维持在不变的鲁棒滤波状态，滤波最优性的损失程度也将不再调整，这对于扰动程度不断变化的场合是难以适用的。因此本发明提出(12)式中的鲁棒切换算法，该算法仅在量测误差方差不大于其实际值时置于鲁棒滤波状态。由此可见，鲁棒切换条件的引入使得鲁棒滤波算法能够合理控制在最优和鲁棒两个工作状态之间。

步骤s33、新息：

步骤s33-1、误差方差矩阵更新：

p(k)＝hσ(k)h^t+v(k)(13)；

式中，σ(k)表示增益调度矩阵，v(k)为系统量测误差方差矩阵。

通过步骤s33-4能够得到第k个时刻的误差方差矩阵；

步骤s32-2、滤波增益矩阵σ(k)更新(无论滤波增益矩阵σ(k)选取上式或下式，均进入式(14))：

k(k)＝σ(k)h^tp^-1(k)(14)；

式中，k(k)表示滤波增益矩阵的更新矩阵；p^-1(k)表示当前时刻k的状态估计矩阵的误差方差矩阵p(k)的逆矩阵；通过k(k)矩阵能够确定状态方程积分与直接量测二者融合的权重矩阵，亦即滤波增益矩阵的更新矩阵k(k)的意义在于确定二者对于状态估计的影响大小；

步骤s32-3、状态估计更新：

此即为第k个时刻的系统状态估计。

步骤s34、算法迭代：

在得到第k个时刻的误差方差矩阵p(k)与状态估计之后，返回步骤s10，重复步骤s10～步骤s32-3，重新获取轨道参数计算相关矩阵(s10、s20)，并进入鲁棒滤波算法流程(s30-s32-3)，直到给出指令，鲁棒滤波算法结束。

步骤s35、误差约束与参数选取：

为保证系统的精度，鲁棒性渐消因子γ与其他参数的设计满足以下准则：

式(16)中，算子maxeig(a)为矩阵a的最大特征值；i为单位矩阵；

在合理选取系统参数的条件下，系统状态估计误差满足以下准则：

其中μ为正的常数且0＜λ＜1，k为当前鲁棒滤波算法所进行的步数，表示系统状态估计误差初值，e{δ}表示变量δ期望，pmin、pmax同样为正常数且满足pmaxi≥p(k)≥pmini，亦即pmin、pmax为误差方差矩阵奇异值上下界，一般情况下选取pmax≥eigmax(p(k))≤eigmin(p(k))即可保证上式成立。

本发明的鲁棒滤波算法本质上放宽了卡尔曼滤波的最优误差方差估计准则，取而代之的是使用误差估计约束条件(16、17)对系统估计误差进行约束，这样做的弊端是模型精度较高条件下系统滤波精度会下降，但优势则是在系统模型不确定性较强、外部扰动较大时系统精度较高。

下面以仿真算例对本专利所提出方法进行说明。在地心惯性系下，假定主星、从星初始位置、速度分别为：

仿真时间与仿真步长为：

t＝200s,tsample＝0.1s(19)；

系统初始状态估计误差为：

系统初始误差方差矩阵为：

同时假定主星以[0.0100]m/s²的加速度做加速运动，则以经典ekf的状态估计结果如图1和图2所示：可以看到相对位置与相对速度估计误差分别为30m与0.5m/s。

同时选取鲁棒滤波参数如下

μ＝0.1,λ＝0.995,α＝0.8,ε＝0.05,γ＝1(22)；

得到经本发明鲁棒滤波算法的仿真结果如图3和图4所示：可以看到相对位置与相对速度估计误差分别为10m与0.5m/s，仿真精度明显优于经典ekf算法。

需要说明的是，在本申请中，诸如第一、第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。

以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。