CN106595715A - 基于捷联惯导/卫星组合导航系统里程计标定方法及装置 - Google Patents

Abstract

本发明涉及一种基于捷联惯导/卫星组合导航系统里程计标定方法及装置，属于捷联惯导技术领域。本发明利用捷联惯导/卫星第一次组合导航的结果反算初始姿态，并利用存储的原始数据进行二次组合导航，用二次组合导航的结果提供标定的参考值，产生“真实”的载体系参考速度，并以该真实速度进行标定。本发明利用捷联惯导/卫星组合导航系统产生标定参考速度，可以得到准确可靠的标定结果。

Description

基于捷联惯导/卫星组合导航系统里程计标定方法及装置

技术领域

本发明涉及一种基于捷联惯导/卫星组合导航系统里程计标定方法及装置，属于捷联惯导技术领域。

背景技术

里程计是测量车辆行驶速度和路程的测量装置，具有自主、测量误差不随时间发散的优点。单独的里程计不具备导航定位功能，但是与惯导系统结合可以以航位推算或组合导航的方式实现高精度、自主导航。组合导航系统在实际使用中，捷联惯导与里程计分别装在载体的不同位置，需要标定里程计的安装角。此外，车轮周长受轮胎温度、充气压力等的影响，需要对里程计的标度因数进行标定。

现有的标定方法主要有两种。一是基于轨迹相似原理(专利CN104180821A)：利用惯性与里程计进行航位推算得到一条轨迹，利用惯性与激光测速仪进行航位推算得到一条轨迹，由这两条轨迹，利用轨迹相似原理直接计算里程计的标定参数。但是由于惯性与激光测速仪得到的轨迹是含有误差的，因此用该方法得到的标定参数必然含有误差。

二是基于惯性/里程计组合导航的标定方法(专利CN104165641A)，即把里程计的安装偏差角及标度因数作为组合导航的误差参数进行在线估计。该方法的缺点在于：里程计误差参数估计效果受载体机动的影响，需要一定的时间收敛，且里程计航向安装偏差角与航向角误差存在一定的耦合。

发明内容

本发明的目的是提供一种基于捷联惯导/卫星组合导航系统里程计标定方法，以解决目前里程计标定方法存在误差的问题；本发明还提供了一种基于捷联惯导/卫星组合导航系统里程计标定装置。

本发明为解决上述技术问题而提供一种基于捷联惯导/卫星组合导航系统的里程计标定方法，包括七个方案，标定方法方案一：该标定方法包括以下步骤：

1)对安装在载体上的惯性导航系统进行初始对准，并记录当前初始参数，包括初始经度、纬度、高度和速度；

2)将惯性导航系统与卫星进行组合导航，并记录导航过程中陀螺、加速度计、卫星导航及里程计的原始数据；

3)在导航结束后，利用当前的姿态反算步骤1)初始对准完成时刻惯性导航系统的姿态；

4)将得到惯性导航系统的初始姿态和步骤1)中记录的初始参数作为组合导航的初值，利用步骤2)储存的原始数据进行二次组合导航，并记录二次组合导航的结果；

5)根据二次组合导航的结果计算载体系下的真实速度序列；

6)利用步骤5)得到速度序列和里程计的速度序列，计算里程计的标定参数。

标定方法方案二：在标定方法方案一的基础上，所述步骤3)中初始对准完成时刻的姿态通过下式计算：

其中n为导航坐标系，b为载体坐标系，i为惯性坐标系，为惯性凝固载体坐标系，为惯性凝固导航坐标系，为当前捷联惯导姿态矩阵，为导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵，为惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵，为初始捷联惯导姿态矩阵。

标定方法方案三：在标定方法方案一的基础上，所述步骤5)中载体系下的真实速度序列y为：

y＝[V^b(1) V^b(2)…V^b(N)]

其中为时刻t_k时导航系速度，为时刻t_k惯性导航系统的姿态，V^b(k)为t_k时刻真实的载体系速度。

标定方法方案四：在标定方法方案一的基础上，所述步骤6)采用最小二乘法计算里程计的标定参数，计算过程如下：

A.分别计算真实速度序列和里程计的速度序列的均值，并计算真实速度序列和里程计的速度序列的协方差矩阵Σ_xy，

其中y(k)为真实速度序列，μ_y为真实速度序列的均值，x(k)为里程计的速度序列，μ_x为里程计的速度序列的均值，N为序列长度；

B.将得到的协方差矩阵进行奇异值SVD分解，以得到矩阵D，

∑_xy＝UDV^T；

C.计算矩阵S，根据矩阵S计算矩阵DS，由矩阵DS计算标度因数k，

其中tr(DS)表示求矩阵DS的迹，为里程计的速度序列的方差。

标定方法方案五：在标定方法方案四的基础上，该方法还包括对里程计三个安装偏差角的计算，三个安装偏差角α、β、γ的计算公式如下：

R＝USV^T

其中R为转换矩阵。

标定方法方案六：在标定方法方案二的基础上，所述的导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵由下式确定：

其中为导航坐标系n到地球坐标系e之间的方向余弦矩阵，为地球系从t到t₀时刻的转换矩阵，λ和L分别为载体所在点的经度和纬度，Δt＝t-t₀为时间间隔，λ₀和L₀分别为初始时刻载体所在点的经度和纬度，ω_ie为地球自转角速度。

标定方法方案七：在标定方法方案二的基础上，所述惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵的初值为一个单位矩阵：

该矩阵利用陀螺输出进行更新：

其中为捷联惯性导航系统陀螺仪的输出。

本发明还提供一种基于捷联惯导/卫星组合导航系统的里程计标定装置，包括五个方案，标定装置方案一：该标定装置包括初始对准模块、一次组合导航模块、初始姿态反算模块、二次组合导航模块、真实速度序列计算模块和标定参数计算模块，

所述的初始对准模块用于对安装在载体上的惯性导航系统进行初始对准，并记录当前初始参数，包括初始经度、纬度、高度和速度；

所述的一次组合导航模块用于将惯性导航系统与卫星进行组合导航，并记录导航过程中陀螺、加速度计、卫星导航及里程计的原始数据；

所述的初始姿态反算模块用于在导航结束后，利用当前的姿态反算初始对准完成时刻惯性导航系统的姿态；

所述的二次组合导航模块用于将得到惯性导航系统的初始姿态和初始对准模块中记录的初始参数作为组合导航的初值，利用储存的原始数据进行二次组合导航，并记录二次组合导航的结果；

所述的真实速度序列计算模块用于根据二次组合导航的结果计算载体系下的真实速度序列；

所述的标定参数计算模块用于根据得到真实速度序列和里程计的速度序列，计算里程计的标定参数。

标定装置方案二：在标定装置方案一的基础上，所述初始姿态反算模块采用的计算公式为：

其中n为导航坐标系，b为载体坐标系，i为惯性坐标系，为惯性凝固载体坐标系，为惯性凝固导航坐标系，为当前捷联惯导姿态矩阵，为导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵，为惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵，为初始捷联惯导姿态矩阵。

标定装置方案三：在标定装置方案一的基础上，所述真实速度序列计算模块计算出的真实速度序列y为：

y＝[V^b(1) V^b(2)…V^b(N)]

其中为时刻t_k时导航系速度，为时刻t_k惯性导航系统的姿态，V^b(k)为t_k时刻真实的载体系速度。

标定装置方案四：在标定装置方案一的基础上，所述的标定参数计算模块采用最小二乘法计算里程计的标定参数，计算过程如下：

A.分别计算真实速度序列和里程计的速度序列的均值，并计算真实速度序列和里程计的速度序列的协方差矩阵Σ_xy，

其中y(k)为真实速度序列，μ_y为真实速度序列的均值，x(k)为里程计的速度序列，μ_x为里程计的速度序列的均值，N为序列长度；

B.将得到的协方差矩阵进行奇异值SVD分解，以得到矩阵D，

∑_xy＝UDV^T；

C.计算矩阵S，根据矩阵S计算矩阵DS，由矩阵DS计算标度因数k，

其中tr(DS)表示求矩阵DS的迹，为里程计的速度序列的方差。

标定装置方案五：在标定装置方案四的基础上，所述的标定参数计算模块还对里程计三个安装偏差角的计算，三个安装偏差角α、β、γ的计算公式如下：

R＝USV^T

其中R为转换矩阵。

本发明的有益效果是:本发明利用捷联惯导/卫星第一次组合导航的结果反算初始姿态，并利用存储的原始数据进行二次组合导航，用二次组合导航的结果提供标定的参考值，产生“真实”的载体系参考速度，并以该真实速度进行标定。本发明利用捷联惯导/卫星组合导航系统产生标定参考速度，可以得到准确可靠的标定结果。

附图说明

图1是本发明捷联惯导/卫星组合导航系统的里程计标定方法的流程图；

图2是捷联惯导/卫星组合导航系统的里程计标定原理框图。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步的说明。

本发明基于捷联惯导/卫星组合导航系统的里程计标定方法的实施例

由于初始对准之后姿态误差仍然需要一定的时间收敛，如果以捷联惯导/卫星第一次组合导航的结果进行标定，以第一次组合导航的速度、姿态计算载体系下的参考速度必然存在一定的误差，从而引起标定结果不准确。为此，发明采用二次组合导航的结果进行标定。本发明以捷联惯导/卫星第一次组合导航的结果反算初始姿态并利用存储的原始数据进行二次组合导航，用二次组合导航的结果提供标定的参考值，产生“真实”的载体系参考速度，利用最小二乘的方式进行标定，可以同时估计标度因数和三个安装偏差角。该方法的流程如图1所示，具体包括以下步骤。

1.将捷联惯性导航系统、卫星导航模块、里程计安装导载体上，系统上电启动。

2.对捷联惯性导航系统进行初始对准，并记录当前初始参数。

本实施例中的初始对准指的是捷联惯性导航系统动基座对准和静基座对准，初始参数包括捷联惯性导航系统初始经度、纬度、高度和速度。

3.将初始对准的捷联惯性导航系统与卫星进行一次组合导航，并在组合导航过程中记录陀螺、加速度计、卫星导航及里程计的原始数据。

4.航程结束后，利用当前的姿态反算步骤2初始对准完成时刻的姿态。

姿态反算的具体实现过程如下：

当前捷联惯性导航系统姿态矩阵可以分解如下：

其中，n为导航坐标系，b为载体坐标系，i为惯性坐标系，惯性凝固载体坐标系它是在t₀时刻将载体坐标系b经惯性凝固后得到，惯性凝固导航坐标系它是在t₀时刻将导航坐标系n经惯性凝固后得到，t₀为起始时刻。式中，可以由以下式子确定：

为导航坐标系n到地球坐标系e之间的方向余弦矩阵，可由载体所在点的经度λ、纬度L确定如下：

地球系从t到t₀时刻的转换矩阵可由时间间隔Δt＝t-t₀确定：

可由初始时刻载体所在点的经度λ₀、纬度L₀确定如下：

为惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵，它的初值是一个单位矩阵：

这个矩阵可以利用陀螺输出进行更新：

其中为导航坐标系n到地球坐标系e之间的方向余弦矩阵，为地球系从t到t₀时刻的转换矩阵，λ和L分别为载体所在点的经度和纬度，Δt＝t-t₀为时间间隔，λ₀和L₀分别为初始时刻载体所在点的经度和纬度，ω_ie为地球自转角速度，为捷联惯性导航系统陀螺仪的输出。

由于坐标系和惯性凝固坐标系,因此它是一个常值，在数值上等于初始时刻的姿态矩阵。在计算得到及之后，根据式(1)即可得到初始的姿态矩阵

5.利用步骤4计算得到的初始姿态、步骤2记录的初始参数(包括初始的经度、纬度、高度、速度)，作为组合导航的初值，利用步骤3储存的原始数据，进行二次组合导航，并记录组合导航的结果(速度、姿态)。

6.利用步骤5二次组合导航的结果(速度、姿态)计算载体系下的真实速度序列。

在二次组合导航中，每一个组合导航解算周期都可以得到导航系下的速度、和态矩阵根据这样两个数值，可以计算得到t_k时刻真实的载体系速度：

在整个标定航程，一共产生了N个速度序列，表示为：

y＝[V^b(1) V^b(2)…V^b(N)] (9)

y为一个3行N列的序列。

7.利用步骤6得到的速度序列和里程计的速度序列，以最小二乘的方法计算里程计的标定参数，原理如图2所示，这里的标定参数包括3个安装偏角和标度因数。具体过程如下：

由步骤6可以得到一个3行N列载体系下的速度序列，里程计速度(V^d)序列也为一个对应的3行N列序列，表示如下：

x＝[V^d(1) V^d(2)…V^d(N)] (10)

标定问题转化为求取这两个点列之间的转换关系，待求的参数含有1个标度因数和3个安装偏差角，这些参数可以通过基于SVD分解的最小二乘算法来求解。

分别计算真实速度序列和里程计的速度序列的均值和方差，并计算真实速度序列和里程计的速度序列的协方差矩阵Σ_xy。

对矩阵Σ_xy进行SVD分解，即：

Σ_xy＝UDV^T (16)

则标度因数k可以由下式计算得到：

其中tr(DS)表示求矩阵DS的迹，矩阵S表示如下

转换矩阵计算如下：

R＝USV^T (19)

转换矩阵与三个安装偏差角α、β、γ的关系：

由式子(20)即可得到三个安装偏差角。

通过上述过程，本发明以捷联惯导/卫星第一次组合导航的结果反算初始姿态，并利用存储的原始数据进行二次组合导航，用二次组合导航的结果提供标定的参考值，产生“真实”的载体系参考速度，利用最小二乘的方式进行标定，所得标定结果准确可靠。

本发明基于捷联惯导/卫星组合导航系统的里程计标定装置的实施例

该标定装置包括初始对准模块、一次组合导航模块、初始姿态反算模块、二次组合导航模块、真实速度序列计算模块和标定参数计算模块，初始对准模块用于对安装在载体上的惯性导航系统进行初始对准，并记录当前初始参数，包括初始经度、纬度、高度和速度；一次组合导航模块用于将惯性导航系统与卫星进行组合导航，并记录导航过程中陀螺、加速度计、卫星导航及里程计的原始数据；初始姿态反算模块用于在导航结束后，利用当前的姿态反算初始对准完成时刻惯性导航系统的姿态；二次组合导航模块用于将得到惯性导航系统的初始姿态和初始对准模块中记录的初始参数作为组合导航的初值，利用储存的原始数据进行二次组合导航，并记录二次组合导航的结果；真实速度序列计算模块用于根据二次组合导航的结果计算载体系下的真实速度序列；标定参数计算模块用于根据得到真实速度序列和里程计的速度序列，计算里程计的标定参数。各模块的具体实现手段已在方法的实施例中进行详细说明，这里不再赘述。

Claims (10)

1.一种基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，该标定方法包括以下步骤：

1)对安装在载体上的惯性导航系统进行初始对准，并记录当前初始参数，包括初始经度、纬度、高度和速度；

2)将惯性导航系统与卫星进行组合导航，并记录导航过程中陀螺、加速度计、卫星导航及里程计的原始数据；

3)在导航结束后，利用当前的姿态反算步骤1)初始对准完成时刻惯性导航系统的姿态；

4)将得到惯性导航系统的初始姿态和步骤1)中记录的初始参数作为组合导航的初值，利用步骤2)储存的原始数据进行二次组合导航，并记录二次组合导航的结果；

5)根据二次组合导航的结果计算载体系下的真实速度序列；

6)利用步骤5)得到速度序列和里程计的速度序列，计算里程计的标定参数。

2.根据权利要求1所述的基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，所述步骤3)中初始对准完成时刻的姿态通过下式计算：

$C_b^n=C_{i_{n_0}}^n{C}_{l_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}$

其中n为导航坐标系，b为载体坐标系，i为惯性坐标系，为惯性凝固载体坐标系，为惯性凝固导航坐标系，为当前捷联惯导姿态矩阵，为导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵，为惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵，为初始捷联惯导姿态矩阵。

3.根据权利要求1所述的基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，所述步骤5)中载体系下的真实速度序列y为：

y＝[V^b(1) V^b(2) … V^b(N)]

$V^b\left(k\right)={\[C_b^n\left(k\right)\]}^T{V}_{INS/GNSS}^n\left(k\right)$

其中为时刻t_k时导航系速度，为时刻t_k惯性导航系统的姿态，V^b(k)为t_k时刻真实的载体系速度。

4.根据权利要求1所述的基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，所述步骤6)采用最小二乘法计算里程计的标定参数，计算过程如下：

A.分别计算真实速度序列和里程计的速度序列的均值，并计算真实速度序列和里程计的速度序列的协方差矩阵Σ_xy，

${\mathrm{\Σ}}_{xy}=\frac{1}{N}\underset{1}{\overset{N}{\Σ}}\left(y\right(k)-{\mathrm{\μ}}_y){\left(x\right(k)-{\mathrm{\μ}}_x)}^T$

其中y(k)为真实速度序列，μ_y为真实速度序列的均值，x(k)为里程计的速度序列，μ_x为里程计的速度序列的均值，N为序列长度；

B.将得到的协方差矩阵进行奇异值SVD分解，以得到矩阵D，

Σ_xy＝UDV^T；

C.计算矩阵S，根据矩阵S计算矩阵DS，由矩阵DS计算标度因数k，

$k=\frac{1}{{\mathrm{\σ}}_x^2}tr\left(DS\right)$

$S=\left\{\begin{array}{cc}I& \left(\right|{\mathrm{\&Sigma;}}_{xy}|>0))\\ diag(1,1...-1)& \left(\right|{\mathrm{\&Sigma;}}_{xy}|<0))\end{array}\right.$

其中tr(DS)表示求矩阵DS的迹，为里程计的速度序列的方差。

5.根据权利要求4所述的基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，该方法还包括对里程计三个安装偏差角的计算，三个安装偏差角α、β、γ的计算公式如下：

$R=\left[\begin{array}{ccc}1& -\mathrm{\&gamma;}& \mathrm{\&beta;}\\ \mathrm{\&gamma;}& 1& -\mathrm{\&alpha;}\\ -\mathrm{\&beta;}& \mathrm{\&alpha;}& 1\end{array}\right]$

R＝USV^T

其中R为转换矩阵。

6.根据权利要求2所述的基于捷联惯导/卫星组合导航系统的里程计标定方法，其特征在于，所述的导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵由下式确定：

$C_{i_{n_0}}^n=C_e^n{C}_{e_0}^e{C}_{i_{n_0}}^{e_0}$

$C_e^n=\left[\begin{array}{ccc}-\sin L\cos \mathrm{\&lambda;}& -\sin L\sin \mathrm{\&lambda;}& \cos L\\ \cos L\cos \mathrm{\&lambda;}& \cos L\sin \mathrm{\&lambda;}& \sin L\\ -\sin \mathrm{\&lambda;}& \cos \mathrm{\&lambda;}& 0\end{array}\right]$

$C_{e_0}^e=\left[\begin{array}{ccc}\cos \left({\mathrm{\&omega;}}_{ie}\mathrm{\&Delta;}t\right)& \sin \left({\mathrm{\&omega;}}_{ie}\mathrm{\&Delta;}t\right)& 0\\ -\sin \left({\mathrm{\&omega;}}_{ie}\mathrm{\&Delta;}t\right)& \cos \left({\mathrm{\&omega;}}_{ie}\mathrm{\&Delta;}t\right)& 0\\ 0& 0& 1\end{array}\right]$

$C_{i_{n_0}}^{e_0}={\&lsqb;C_{e_0}^{i_{n_0}}\&rsqb;}^T={\left[\begin{array}{ccc}-\sin L_0{\mathrm{cos\&lambda;}}_0& -\sin L_0{\mathrm{sin\&lambda;}}_0& \cos L_0\\ \cos L_0{\mathrm{cos\&lambda;}}_0& \cos L_0{\mathrm{sin\&lambda;}}_0& \sin L_0\\ -{\mathrm{sin\&lambda;}}_0& {\mathrm{cos\&lambda;}}_0& 0\end{array}\right]}^T$

其中为导航坐标系n到地球坐标系e之间的方向余弦矩阵，为地球系从t到t₀时刻的转换矩阵，λ和L分别为载体所在点的经度和纬度，Δt＝t-t₀为时间间隔，λ₀和L₀分别为初始时刻载体所在点的经度和纬度，ω_ie为地球自转角速度。

7.一种基于捷联惯导/卫星组合导航系统的里程计标定装置，其特征在于，该标定装置包括初始对准模块、一次组合导航模块、初始姿态反算模块、二次组合导航模块、真实速度序列计算模块和标定参数计算模块，

所述的初始对准模块用于对安装在载体上的惯性导航系统进行初始对准，并记录当前初始参数，包括初始经度、纬度、高度和速度；

所述的一次组合导航模块用于将惯性导航系统与卫星进行组合导航，并记录导航过程中陀螺、加速度计、卫星导航及里程计的原始数据；

所述的初始姿态反算模块用于在导航结束后，利用当前的姿态反算初始对准完成时刻惯性导航系统的姿态；

所述的二次组合导航模块用于将得到惯性导航系统的初始姿态和初始对准模块中记录的初始参数作为组合导航的初值，利用储存的原始数据进行二次组合导航，并记录二次组合导航的结果；

所述的真实速度序列计算模块用于根据二次组合导航的结果计算载体系下的真实速度序列；

所述的标定参数计算模块用于根据得到真实速度序列和里程计的速度序列，计算里程计的标定参数。

8.根据权利要求7所述的基于捷联惯导/卫星组合导航系统的里程计标定装置，其特征在于，所述初始姿态反算模块采用的计算公式为：

$C_b^n=C_{i_{n_0}}^n{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}$

其中n为导航坐标系，b为载体坐标系，i为惯性坐标系，为惯性凝固载体坐标系，为惯性凝固导航坐标系，为当前捷联惯导姿态矩阵，为导航坐标系n和惯性凝固导航坐标系之间的方向余弦矩阵，为惯性凝固坐标系和载体坐标系b之间的方向余弦矩阵，为初始捷联惯导姿态矩阵。

9.根据权利要求7所述的基于捷联惯导/卫星组合导航系统的里程计标定装置，其特征在于，所述真实速度序列计算模块计算出的真实速度序列y为：

y＝[V^b(1) V^b(2) … V^b(N)]

$V^b\left(k\right)={\&lsqb;C_b^n\left(k\right)\&rsqb;}^T{V}_{INS/GNSS}^n\left(k\right)$

其中为时刻t_k时导航系速度，为时刻t_k惯性导航系统的姿态，V^b(k)为t_k时刻真实的载体系速度。

10.根据权利要求7所述的基于捷联惯导/卫星组合导航系统的里程计标定装置，其特征在于，所述的标定参数计算模块采用最小二乘法计算里程计的标定参数，计算过程如下：

A.分别计算真实速度序列和里程计的速度序列的均值，并计算真实速度序列和里程计的速度序列的协方差矩阵Σ_xy，

${\mathrm{\&Sigma;}}_{xy}=\frac{1}{N}\underset{1}{\overset{N}{\&Sigma;}}\left(y\right(k)-{\mathrm{\&mu;}}_y){\left(x\right(k)-{\mathrm{\&mu;}}_x)}^T$

其中y(k)为真实速度序列，μ_y为真实速度序列的均值，x(k)为里程计的速度序列，μ_x为里程计的速度序列的均值，N为序列长度；

B.将得到的协方差矩阵进行奇异值SVD分解，以得到矩阵D，

Σ_xy＝UDV^T；

C.计算矩阵S，根据矩阵S计算矩阵DS，由矩阵DS计算标度因数k，

$k=\frac{1}{{\mathrm{\&sigma;}}_x^2}tr\left(DS\right)$

$S=\left\{\begin{array}{cc}I& \left(\right|{\mathrm{\&Sigma;}}_{xy}|>0))\\ diag(1,1...-1)& \left(\right|{\mathrm{\&Sigma;}}_{xy}|<0))\end{array}\right.$

其中tr(DS)表示求矩阵DS的迹，为里程计的速度序列的方差。