一种实现卡尔曼滤波定位的方法和装置与流程

本发明涉及但不限于导航定位领域，尤指一种实现卡尔曼滤波定位的方法和装置。

背景技术：

全球导航卫星系统(gnss，globalnavigationsatellitesystem)已经广泛应用于多个领域。目前，主要有四个全球导航定位系统，包括美国的全球定位系统(gps，globalpositioningsystem)、中国的北斗卫星导航系统(compass或beidoubd)、俄罗斯的全球导航卫星系统(glonass，globalnavigationsatellitesystem)、欧盟推出的规模最大的民用卫星导航定位系统，即伽利略(galileo)系统。其中，gps发展最为成熟，bd和glonass已经取得阶段性进展，而伽利略系统还处于起步阶段。

导航定位过程中用户最为关心的是定位、定速和定时的结果，那么对用户定位，定速和定时的算法和技术就显得尤为重要。目前，导航系统中主要的定位、定速和定时的算法有最小二乘定位和卡尔曼滤波定位。最小二乘定位是孤立的求解每一个不同时刻的系统状态，而卡尔曼滤波定位利用状态方程将不同时刻的系统状态联系起来，使对系统的跟踪轨迹更为平滑。

在卡尔曼滤波定位系统中用状态方程描述每一时刻用户的运动特性，而对系统状态向量校正时，涉及到了对测量值的预测和卡尔曼增益的计算。如果状态方程和测量方程是非线性的，还应对其进行线性化计算。

相关技术中，实现卡尔曼滤波定位的方法大致包括：

获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵；

将状态向量的初始值的估计值作为上一个历元时状态向量的预测值计算当前历元时状态向量的预测值；将状态向量的初始值的估计值的误差协方差矩阵作为上一个历元时误差协方差矩阵计算当前历元时误差协方差矩阵；

根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵；

继续执行将当前历元时状态向量的最优估计值作为当前历元时状态向量的预测值计算下一个历元时状态向量的预测值；将当前历元时状态向量的最优估计值的误差协方差矩阵作为当前历元时误差协方差矩阵计算下一个历元时误差协方差矩阵的步骤。

相关技术中，当系统状态向量的维数较大时，需要的测量方程就会变得越多，大大的增加了计算卡尔曼增益时相关矩阵的计算量，进而极大的增加了定位系统的功耗。

技术实现要素：

为了解决上述问题，本发明实施例提出了一种实现卡尔曼滤波定位的方法和装置，能够减少计算量，从而降低计算所需要的功耗。

为了达到上述目的，本发明实施例提出了一种实现卡尔曼滤波定位的方法，包括：

根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵；

对于当前历元时状态向量的最优估计值中的每一个元素，计算该元素上上一个历元时的最优估计值和当前历元时的最优估计值之间的平均值，和上一个历元时的最优估计值之间的差值；

判断出一个或多个元素对应的计算得到的差值小于或等于预设阈值，将当前历元时状态向量的最优估计值中删除一个或多个元素后的状态向量作为当前历元时状态向量的预测值，将当前历元时状态向量的最优估计值的误差协方差矩阵中删除对应的一个或多个元素后的误差协方差矩阵作为当前历元时误差协方差矩阵，继续执行根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算下一个历元时状态向量的最优估计值和下一个历元时状态向量的最优估计值的误差协方差矩阵的步骤。

可选的，当判断出所有元素对应的计算得到的差值均大于预设阈值时，该方法还包括：

继续执行所述根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵的步骤。

可选的，所述根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵包括：

根据所述上一个历元时状态向量的预测值计算当前历元时状态向量的预测值；根据所述上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵；

根据所述当前历元时状态向量的预测值和所述当前历元时误差协方差矩阵计算所述当前历元时状态向量的最优估计值和所述当前历元时状态向量的最优估计值的误差协方差矩阵。

可选的，该方法之前还包括：

获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵；

所述根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值包括：

将所述状态向量的初始值的估计值作为所述上一个历元时状态向量的预测值计算所述当前历元时状态向量的预测值；

所述根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵包括：

将所述状态向量的初始值的估计值的误差协方差矩阵作为所述上一个历元时误差协方差矩阵计算所述当前历元时误差协方差矩阵。

可选的，所述将状态向量的初始值的估计值作为上一个历元时状态向量的预测值计算当前历元时状态向量的预测值包括：

按照公式计算第k历元时状态向量的预测值；

其中，所述k为历元时数，为所述第k历元时状态向量的预测值，为第(k-1)历元时状态向量的预测值，uk-1为第(k-1)历元时的输入量，wk-1为第(k-1)历元时状态向量的状态变量之间进行转换过程中的噪声，a为状态转移矩阵，b为输入关系矩阵。

可选的，所述将状态向量的初始值的估计值的误差协方差矩阵作为上一个历元时误差协方差矩阵计算当前历元时误差协方差矩阵包括：

按照公式计算第k历元时误差协方差矩阵；

其中，为所述第k历元时误差协方差矩阵，为第(k-1)历元时误差协方差矩阵，q为wk-1的误差协方差矩阵。

可选的，所述根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵包括：

根据所述当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值；

根据所述当前历元时误差协方差矩阵计算当前历元时卡尔曼增益；

根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余；

根据所述当前历元时状态向量的预测值、计算得到的当前历元时残余和当前历元时卡尔曼增益计算所述当前历元时状态向量的最优估计值，根据所述计算得到的当前历元时卡尔曼增益和所述当前历元时误差协方差矩阵计算所述当前历元时状态向量的最优估计值的误差协方差矩阵。

可选的，所述根据当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值包括：

按照公式计算所述n个卫星中第i个卫星的校正后的伪距的预测值；

按照公式计算所述n个卫星中第i个卫星的校正后的伪距变化率的预测值；

其中，为所述n个卫星中第n个卫星的校正后的伪距的预测值；(x⁽ⁱ⁾,y⁽ⁱ⁾,z⁽ⁱ⁾)为所述n个卫星中第i个卫星的空间位置坐标，(x,y,z)为所述状态向量中用户的位置坐标的预测值，δtu为所述状态向量中接收机时钟钟差的预测值，为所述n个卫星中第i个卫星的伪距测量误差项，为所述n个卫星中第i个卫星的校正后的伪距变化率的预测值，为所述n个卫星中第i个卫星在空间直角坐标系中三个方向的速度组成的向量，v＝(vx,vy,vz)为所述状态向量中用户在空间直角坐标系中三个方向上的速度的预测值组成的向量，δfu为所述状态向量中接收机时钟的频漂的预测值，为所述n个卫星中第i个卫星的伪距测量误差项的变化率。

可选的，所述根据当前历元时误差协方差矩阵计算当前历元时卡尔曼增益包括：

按照公式计算所述第k历元时卡尔曼增益；

其中，kk为所述第k历元时卡尔曼增益，c为对公式进行线性化得到的雅可比矩阵，r为校正后的伪距的预测值的误差协方差矩阵。

可选的，所述根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余包括：

按照公式计算n个卫星的校正后的伪距和伪距变化率的残余；

其中，ρc为所述n个卫星的校正后的伪距的真实值，为所述计算得到的n个卫星的校正后的伪距的预测值，为所述n个卫星的校正后的伪距变化率的真实值，为所述计算得到的n个卫星的校正后的伪距变化率的预测值。

可选的，所述根据所述当前历元时状态向量的预测值、计算得到的当前历元时残余和当前历元时卡尔曼增益计算所述当前历元时状态向量的最优估计值包括：

按照公式计算第k历元时状态向量的最优估计值；

其中，为所述第k历元时状态向量的最优估计值，β为所述n个卫星的校正后的伪距和伪距变化率的残余。

可选的，所述根据所述计算得到的当前历元时卡尔曼增益和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值的误差协方差矩阵包括：

按照公式计算第k历元时状态向量的最优估计值的误差协方差矩阵；

其中，pk为所述第k历元时状态向量的最优估计值的误差协方差矩阵，为所述n个卫星中第i个卫星的校正后的伪距变化率的真实值，λ⁽ⁱ⁾为所述n个卫星中第i个卫星的载波信号的波长，为所述n个卫星中第i个卫星与用户之间的多普勒频移。

本发明实施例还提出了一种实现卡尔曼滤波定位的装置，包括：

第一计算模块，用于根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵；判断出一个或多个元素对应的计算得到的差值小于或等于预设阈值，将当前历元时状态向量的最优估计值中删除一个或多个元素后的状态向量作为当前历元时状态向量的预测值，将当前历元时状态向量的最优估计值的误差协方差矩阵中删除对应的一个或多个元素后的误差协方差矩阵作为当前历元时误差协方差矩阵，继续执行根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算下一个历元时状态向量的最优估计值和下一个历元时状态向量的最优估计值的误差协方差矩阵的步骤；

第二计算模块，用于对于当前历元时状态向量的最优估计值中的每一个元素，计算该元素上上一个历元时的最优估计值和当前历元时的最优估计值之间的平均值，和上一个历元时的最优估计值之间的差值。

可选的，所述第一计算模块还用于：

判断出所有元素对应的计算得到的差值均大于预设阈值，继续执行所述根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵的步骤。

可选的，所述第一计算模块具体用于采用以下方式实现根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵：

根据所述上一个历元时状态向量的预测值计算当前历元时状态向量的预测值；根据所述上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵；

根据所述当前历元时状态向量的预测值和所述当前历元时误差协方差矩阵计算所述当前历元时状态向量的最优估计值和所述当前历元时状态向量的最优估计值的误差协方差矩阵。

可选的，还包括：

获取模块，用于获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵；

所述第一计算模块具体用于采用以下方式实现所述根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值：

将所述状态向量的初始值的估计值作为所述上一个历元时状态向量的预测值计算所述当前历元时状态向量的预测值；

所述第一计算模块具体用于采用以下方式实现所述根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵：

将所述状态向量的初始值的估计值的误差协方差矩阵作为所述上一个历元时误差协方差矩阵计算所述当前历元时误差协方差矩阵。

可选的，所述第一计算模块具体用于采用以下方式实现根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵：

根据所述当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值；

根据所述当前历元时误差协方差矩阵计算当前历元时卡尔曼增益；

根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余；

根据所述当前历元时状态向量的预测值、计算得到的当前历元时残余和当前历元时卡尔曼增益计算所述当前历元时状态向量的最优估计值，根据所述计算得到的当前历元时卡尔曼增益和所述当前历元时误差协方差矩阵计算所述当前历元时状态向量的最优估计值的误差协方差矩阵。

与现有技术相比，本发明实施例的技术方案包括：根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵；对于当前历元时状态向量的最优估计值中的每一个元素，计算该元素上上一个历元时的最优估计值和当前历元时的最优估计值之间的平均值，和上一个历元时的最优估计值之间的差值；判断出一个或多个元素对应的计算得到的差值小于或等于预设阈值，将当前历元时状态向量的最优估计值中删除一个或多个元素后的状态向量作为当前历元时状态向量的预测值，将当前历元时状态向量的最优估计值的误差协方差矩阵中删除对应的一个或多个元素后的误差协方差矩阵作为当前历元时误差协方差矩阵，继续执行根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算下一个历元时状态向量的最优估计值和下一个历元时状态向量的最优估计值的误差协方差矩阵的步骤。通过本发明实施例的方案，判断出当前历元时状态向量的最优估计值中的一个或多个元素对应的计算得到的差值小于或等于预设阈值时，将一个或多个元素删除后再继续进行计算，减少了状态向量的维度，从而减少了计算量，降低了计算所需要的功耗。

附图说明

下面对本发明实施例中的附图进行说明，实施例中的附图是用于对本发明的进一步理解，与说明书一起用于解释本发明，并不构成对本发明保护范围的限制。

图1为本发明实施例实现卡尔曼滤波定位的方法的流程图；

图2为本发明实施例实现卡尔曼滤波定位的装置的结构组成示意图。

具体实施方式

为了便于本领域技术人员的理解，下面结合附图对本发明作进一步的描述，并不能用来限制本发明的保护范围。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的各种方式可以相互组合。

参见图1，本发明实施例提出了一种实现卡尔曼滤波定位的方法，包括：

步骤100、根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵。具体包括：

根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值；根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵；根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵。

本步骤中，当用户处于静止状态时，状态向量包括用户的位置坐标和接收机时钟钟差，当用户处于运动状态时，状态向量还包括用户在直角坐标系中各方向上的速度和接收机时钟的频漂。

其中，根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值包括：

按照公式(1)计算第k历元时状态向量的预测值。

其中，k为历元时数，为第k历元时状态向量的预测值，为第(k-1)历元时状态向量的预测值；

uk-1为第(k-1)历元时的输入量，但并不是所有的系统都有输入量，比如gps接收机定位系统就没有任何输入量，如果有l个输入量，那么uk是一个l×1的向量；

wk-1为第(k-1)历元时状态向量的状态变量之间进行转换过程中的噪声，在卡尔曼滤波定位系统中假设wk-1是一个均值为零的白噪声，即e(wk-1)＝0,cov(wk-1)＝e(wk-1wk-1^t)＝q，q为wk-1的误差协方差矩阵，为n×n的对称矩阵，对于定位系统来说wk是未知的，但其误差协方差矩阵q是已知的；n为状态向量中状态变量的个数；

a为n×n的状态转移矩阵，可以根据位移方程确定；

b为n×l的输入关系矩阵。

例如，当状态向量包括用户的位置坐标、接收机时钟钟差、用户在直角坐标系中各方向上的速度和接收机时钟的频漂时，

其中，根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵包括：

按照公式计算第k历元时误差协方差矩阵。

其中，为第k历元时误差协方差矩阵，为第(k-1)历元时误差协方差矩阵。

其中，根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵包括：

根据当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值；

根据当前历元时误差协方差矩阵计当前历元时算卡尔曼增益；

根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余；

根据当前历元时状态向量的预测值、计算得到的残余和当前历元时卡尔曼增益计算当前历元时状态向量的最优估计值，根据计算得到的当前历元时卡尔曼增益和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值的误差协方差矩阵。

其中，根据当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值包括：

按照公式(2)计算n个卫星中第i个卫星的校正后的伪距的预测值。

其中，为n个卫星中第i个卫星的校正后的伪距的预测值；(x⁽ⁱ⁾,y⁽ⁱ⁾,z⁽ⁱ⁾)为n个卫星中第i个卫星的空间位置坐标，(x,y,z)为状态向量中用户的位置坐标的预测值，δtu为状态向量中接收机时钟钟差的预测值，为n个卫星中第i个卫星的伪距测量误差项。

按照公式(3)计算n个卫星中第i个卫星的校正后的伪距变化率的预测值。

其中，为n个卫星中第i个卫星的校正后的伪距变化率的预测值，为n个卫星中第i个卫星在空间直角坐标系中三个方向的速度组成的向量，v＝(vx,vy,vz)为状态向量中用户在空间直角坐标系中三个方向上的速度的预测值组成的向量，δfu为状态向量中接收机时钟的频漂的预测值，为n个卫星中第i个卫星的伪距测量误差项的变化率。

其中，根据当前历元时误差协方差矩阵计算当前历元时卡尔曼增益包括：

按照公式(4)计算卡尔曼增益。

其中，kk为第k历元时卡尔曼增益，c为对公式(2)进行线性化得到的雅可比矩阵，r为校正后的伪距的预测值的误差协方差矩阵。

其中，根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余包括：

按照公式计算n个卫星的校正后的伪距和伪距变化率的残余。

其中，ρc为n个卫星的校正后的伪距的真实值，为计算得到的n个卫星的校正后的伪距的预测值，为n个卫星的校正后的伪距变化率的真实值，为计算得到的n个卫星的校正后的伪距变化率的预测值。

其中，根据当前历元时状态向量的预测值、计算得到的残余和卡尔曼增益计算当前历元时状态向量的最优估计值包括：

按照公式(5)计算第k历元时状态向量的最优估计值。

其中，为第k历元时状态向量的最优估计值，β为n个卫星的校正后的伪距和伪距变化率的残余。

其中，根据计算得到的卡尔曼增益和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值的误差协方差矩阵包括：

按照公式(6)计算第k历元时状态向量的最优估计值的误差协方差矩阵。

其中，pk为第k历元时状态向量的最优估计值的误差协方差矩阵。

其中，为n个卫星中第i个卫星的校正后的伪距变化率的真实值，λ⁽ⁱ⁾为n个卫星中第i个卫星的载波信号的波长，为n个卫星中第i个卫星与用户之间的多普勒频移。

步骤101、对于当前历元时状态向量的最优估计值中的每一个元素，计算该元素上上一个历元时的最优估计值和当前历元时的最优估计值之间的平均值，和上一个历元时的最优估计值之间的差值。

步骤102、判断出一个或多个元素对应的计算得到的差值小于或等于预设阈值，将当前历元时状态向量的最优估计值中删除一个或多个元素后的状态向量作为当前历元时状态向量的预测值，将当前历元时状态向量的最优估计值的误差协方差矩阵中删除对应的一个或多个元素后的误差协方差矩阵作为当前历元时误差协方差矩阵，继续执行根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算下一个历元时状态向量的最优估计值和下一个历元时状态向量的最优估计值的误差协方差矩阵的步骤。

当判断出所有元素对应的计算得到的差值均大于预设阈值时，该方法还包括：

继续执行根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵的步骤。

可选的，该方法之前还包括：

获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵；相应地，步骤100中，

根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值包括：

将状态向量的初始值的估计值作为上一个历元时状态向量的预测值计算所述当前历元时状态向量的预测值；

根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵包括：

将状态向量的初始值的估计值的误差协方差矩阵作为上一个历元时误差协方差矩阵计算所述当前历元时误差协方差矩阵。

其中，获得的状态向量的初始值的估计值满足获得的状态向量的初始值的估计值的误差协方差矩阵满足其中，为状态向量的初始值的估计值，x0为状态向量的初始值的真实值，p0为状态向量的初始值的估计值的误差协方差矩阵。

如果其中有一个不满足上述条件，则需要重新获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵，具体如何获取可以采用本领域技术人员的熟知技术实现，并不用于限定本发明的保护范围，这里不再赘述。

通过本发明实施例的方案，判断出当前历元时状态向量的最优估计值中的一个或多个元素对应的计算得到的差值小于或等于预设阈值时，将一个或多个元素删除后再继续进行计算，减少了状态向量的维度，从而减少了计算量，降低了计算所需要的功耗。

参见图2、本发明实施例还提出了一种实现卡尔曼滤波定位的装置，包括：

第一计算模块，用于根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵；判断出一个或多个元素对应的计算得到的差值小于或等于预设阈值，将当前历元时状态向量的最优估计值中删除一个或多个元素后的状态向量作为当前历元时状态向量的预测值，将当前历元时状态向量的最优估计值的误差协方差矩阵中删除对应的一个或多个元素后的误差协方差矩阵作为当前历元时误差协方差矩阵，继续执行根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算下一个历元时状态向量的最优估计值和下一个历元时状态向量的最优估计值的误差协方差矩阵的步骤；

第二计算模块，用于对于当前历元时状态向量的最优估计值中的每一个元素，计算该元素上上一个历元时的最优估计值和当前历元时的最优估计值之间的平均值，和上一个历元时的最优估计值之间的差值。

本发明实施例的装置中，第一计算模块还用于：

判断出所有元素对应的计算得到的差值均大于预设阈值，继续执行根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵的步骤。

本发明实施例的装置中，第一计算模块具体用于采用以下方式实现根据上一个历元时状态向量的预测值和上一个历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵：

根据所述上一个历元时状态向量的预测值计算当前历元时状态向量的预测值；根据所述上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵；

根据所述当前历元时状态向量的预测值和所述当前历元时误差协方差矩阵计算所述当前历元时状态向量的最优估计值和所述当前历元时状态向量的最优估计值的误差协方差矩阵。

本发明实施例的装置中，还包括：

获取模块，用于获取状态向量的初始值的估计值和状态向量的初始值的估计值的误差协方差矩阵；

所述第一计算模块具体用于采用以下方式实现所述根据上一个历元时状态向量的预测值计算当前历元时状态向量的预测值：

将所述状态向量的初始值的估计值作为所述上一个历元时状态向量的预测值计算所述当前历元时状态向量的预测值；

所述第一计算模块具体用于采用以下方式实现所述根据上一个历元时误差协方差矩阵计算当前历元时误差写方差矩阵：

将所述状态向量的初始值的估计值的误差协方差矩阵作为所述上一个历元时误差协方差矩阵计算所述当前历元时误差协方差矩阵。

本发明实施例的装置中，第一计算模块具体用于采用以下方式实现根据当前历元时状态向量的预测值和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值和当前历元时状态向量的最优估计值的误差协方差矩阵：

根据当前历元时状态向量的预测值计算n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值；

根据当前历元时误差协方差矩阵计算当前历元时卡尔曼增益；

根据计算得到的n个卫星的校正后的伪距的预测值和n个卫星的校正后的伪距变化率的预测值计算n个卫星的校正后的伪距和伪距变化率的残余；

根据当前历元时状态向量的预测值、计算得到的当前历元时残余和当前历元时卡尔曼增益计算当前历元时状态向量的最优估计值，根据计算得到的当前历元时卡尔曼增益和当前历元时误差协方差矩阵计算当前历元时状态向量的最优估计值的误差协方差矩阵。

需要说明的是，以上所述的实施例仅是为了便于本领域的技术人员理解而已，并不用于限制本发明的保护范围，在不脱离本发明的发明构思的前提下，本领域技术人员对本发明所做出的任何显而易见的替换和改进等均在本发明的保护范围之内。