基于坐标系传递的姿态估计方法、系统、终端、介质及应用

摘要

本发明属于惯性导航技术领域，公开了一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；进行UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；进入滤波流程。本发明能充分发挥UKF在非线性状态估计中的优势，实现对载体姿态信息的有效估计。从坐标系的角度明确姿态误差的含义，避免了传统以位置作为姿态误差区分而引起的含义不明确的问题。

权利要求

1.一种基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)‑UKF姿态估计方法包括：通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递；

构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果；

所述基于坐标系传递的SO(3)‑UKF姿态估计方法具体包括以下步骤：步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。

2.如权利要求1所述基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系 包括：建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；

建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；

建立估计参考坐标系 所述估计参考坐标系 为SO(3)‑UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系 与所述参考坐标系r之间的误差即为姿态误差。

3.如权利要求2所述基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，步骤二中，所述姿态误差定义如下：其中， 表示真实的误差矩阵， 表示 对应的估计值；

所述进行sigma点采样并构造全局姿态所对应的sigma点包括：利用上一时刻姿态方差Pk‑1进行sigma点采样如下：所述构造全局姿态所对应的sigma点包括：将姿态误差sigma点转换为矩阵形式：

其中，so3_exp[·]所定义的转换公式为：利用上一时刻的姿态估计值 构造全局姿态所对应的sigma点

4.如权利要求3所述基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：其中，Ω(·)表示姿态运动学方程；

步骤三中，所述构造传递后的姿态误差sigma点；

根据定义的姿态误差，构造传递后的姿态误差sigma点：将矩阵形式的姿态误差转换为矢量形式的误差：其中，so3_log[·]所定义的转换公式为：

5.如权利要求4所述基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，步骤四中，所述进行常规的UKF预测及预测方差计算如下：其中，w(i)表示sigma点所对应的权值，Qk‑1表示状态方差阵；

步骤四中，所述分别进行UKF量测更新以及全局姿态更新包括：所述进行UKF量测更新包括：

通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；

所述进行全局姿态更新包括：

将矢量姿态误差估计 转换为矩阵形式

根据姿态误差定义，得到当前时刻的全局姿态更新:所述姿态误差估计置零如下：

6.如权利要求5所述基于坐标系传递的SO(3)‑UKF姿态估计方法，其特征在于，所述基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

所述基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：所述全局姿态采用四元数进行表示；所述姿态误差采用三维表示方法进行表示。

7.一种基于坐标系传递的SO(3)‑UKF姿态估计系统，其特征在于，所述基于坐标系传递的SO(3)‑UKF姿态估计系统包括：坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

滤波模块，用于进入下一时刻的滤波流程。

8.一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行权利要求1～6任意一项所述基于坐标系传递的SO(3)‑UKF姿态估计方法。

9.一种如权利要求1～6任意一项所述基于坐标系传递的SO(3)‑UKF姿态估计方法在运载体导航、机器人控制、航天器交会对接领域上的应用。

说明书

基于坐标系传递的姿态估计方法、系统、终端、介质及应用

技术领域

[0001] 本发明属于惯性导航技术领域，尤其涉及一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用。

背景技术

[0002] 目前，利用固连于载体的惯性器件及其它传感器信息估计载体的姿态信息在运载体导航、机器人控制、航天器交会对接等领域具有广泛的应用。UKF(Unscented Kalman 
Filter)因其适中的计算量、较高的估计精度，近年来正逐步取代传统的EKF(Extended 
Kalman Filter)而广泛应用于非线性估计领域。然而将UKF应用姿态估计问题中则需要专
门处理姿态约束在UKF滤波加权平均运算中的保持问题。针对这一问题，常规的做法是利用冗余姿态表示如四元数、方向余弦矩阵等进行全局姿态更新，利用三维姿态表示如旋转矢
量、罗德里格斯参数等进行滤波估计，这种方法一般称之为SO(3)‑UKF。在SO(3)‑UKF中，UKF预测和估计的状态量本质上是三维姿态误差矢量，因此该算法设计的核心在于姿态误差的
定义及传递。在传统的SO(3)‑UKF算法设计中，多以“左”和“右”来区分不同的姿态误差定义。然而，从坐标系定义角度来讲，由于不同的姿态定义，“左”误差既可以表示载体系误差也可以表示参考系误差。因此，单纯的以位置来表示姿态误差并不严谨。同时，同从坐标系定义角度来讲，相同位置的误差也可能表示不同的含义。这是因为，根据坐标系定义，载体的姿态表示从一个坐标系到另一个坐标系的传递，因此其对应的误差共有四种形式。不同
误差的定义影响着SO(3)‑UKF中sigma点的采样、传递、重构以及全局姿态的更新。

[0003] 通过上述分析，现有技术存在的问题及缺陷为：现有姿态估计方法本以位置作为姿态误差区分导致含义不明确且坐标系不明确导致的执行过程不统一。

发明内容

[0004] 针对现有技术存在的问题，本发明提供了一种基于坐标系传递的姿态估计方法、系统、终端、介质及应用，具体涉及一种基于坐标系传递的SO(3)‑UKF姿态估计方法。

[0005] 本发明是这样实现的，一种基于坐标系传递的SO(3)‑UKF姿态估计方法，包括：

[0006] 通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递，构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。

[0007] 进一步，所述基于坐标系传递的SO(3)‑UKF姿态估计方法包括以下步骤：

[0008] 步骤一，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系

[0009] 步骤二，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

[0010] 步骤三，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

[0011] 步骤四，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

[0012] 步骤五，进入下一时刻的滤波流程，重复步骤一至步骤四。

[0013] 进一步，步骤一中，所述建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系 包括：

[0014] 建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；

[0015] 建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；

[0016] 建立估计参考坐标系 所述估计参考坐标系 为SO(3)‑UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系 与所述参考坐标系r之间的误差即为姿态误差。

[0017] 进一步，步骤二中，所述姿态误差定义如下：

[0018]

[0019] 其中， 表示真实的误差矩阵， 表示 对应的估计值。

[0020] 进一步，步骤二中，所述进行sigma点采样并构造全局姿态所对应的sigma点包括：

[0021] 利用上一时刻姿态方差Pk‑1进行sigma点采样如下：

[0022]

[0023] 所述构造全局姿态所对应的sigma点包括：

[0024] 将姿态误差sigma点转换为矩阵形式：

[0025]

[0026] 其中，so3_exp[·]所定义的转换公式为：

[0027]

[0028] 利用上一时刻的姿态估计值 构造全局姿态所对应的sigma点

[0029]

[0030] 进一步，步骤三中，所述将姿态sigma点在姿态运动学方程中传递包括：

[0031]

[0032] 其中，Ω(·)表示姿态运动学方程。

[0033] 进一步，步骤三中，所述构造传递后的姿态误差sigma点；

[0034] 根据定义的姿态误差，构造传递后的姿态误差sigma点：

[0035]

[0036] 将矩阵形式的姿态误差转换为矢量形式的误差：

[0037]

[0038] 其中，so3_log[·]所定义的转换公式为：

[0039]

[0040] 进一步，步骤四中，所述进行常规的UKF预测及预测方差计算如下：

[0041]

[0042]

[0043] 其中，w(i)表示sigma点所对应的权值，Qk‑1表示状态方差阵。

[0044] 进一步，步骤四中，所述分别进行UKF量测更新以及全局姿态更新包括：所述进行UKF量测更新包括：

[0045] 通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即

[0046]

[0047] 其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；所述进行全局姿态更新包括：

[0048] 将矢量姿态误差估计 转换为矩阵形式

[0049]

[0050] 根据姿态误差定义，得到当前时刻的全局姿态更新:

[0051]

[0052] 进一步，所述姿态误差估计置零如下：

[0053]

[0054] 进一步，所述基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：

[0055] 当姿态误差定义为 时，姿态sigma点构造为传递后的姿态误差sigma点为 姿态更新为

[0056] 当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

[0057] 当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

[0058] 进一步，所述基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：

[0059] 所述全局姿态可用四元数进行表示；所述姿态误差可用其它三维表示方法进行表示。

[0060] 本发明另一目的在于提供一种基于坐标系传递的SO(3)‑UKF姿态估计系统，包括：

[0061] 坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系

[0062] 全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

[0063] 传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

[0064] 更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

[0065] 滤波模块，用于进入下一时刻的滤波流程。

[0066] 本发明另一目的在于提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行所述基于坐标系传递的SO(3)‑UKF姿态估计方法。

[0067] 本发明另一目的在于提供一种所述基于坐标系传递的SO(3)‑UKF姿态估计方法在运载体导航、机器人控制、航天器交会对接领域上的应用。

[0068] 结合上述的所有技术方案，本发明所具备的优点及积极效果为：

[0069] 本发明从坐标系角度定义特定的姿态误差，并从坐标系传递的角度对SO(3)‑UKF算法流程中的sigma点采样、传递、重构以及全局姿态更新进行统一的坐标系约束和规范。
根据特定的姿态误差定义，本发明给出了详细的SO(3)‑UKF算法设计流程。利用本发明，从坐标系传递的角度统一sigma点采样、传递、重构以及姿态更新所涉及的姿态信息计算，从而结合实际应用问题中具体的姿态含义，高效的设计出相应的SO(3)‑UKF算法，进而充分发挥UKF在非线性状态估计中的优势，实现对载体姿态信息的有效估计。

[0070] 本发明从坐标系的角度明确姿态误差的含义，避免了传统以位置作为姿态误差区分而引起的含义不明确的问题；

[0071] 本发明以坐标系传递约束SO(3)‑UKF中sigma点采样、传递、重构以及全局姿态更新，从而避免了传统方法中因坐标系不明确而导致的上述执行过程不统一的问题；

[0072] 本发明提供了四种不同的姿态定义方式，并给出了SO(3)‑UKF相应的设计流程，从而可以针对实际应用问题、结合具体应用需求高效的设计出相应的滤波算法。

附图说明

[0073] 图1是本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法原理图。

[0074] 图2是本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法流程图。

[0075] 图3是本发明实施例提供的仿真实验轨迹图。

[0076] 图4是本发明实施例提供的位置误差曲线图。

[0077] 图5是本发明实施例提供的方位误差曲线图。

具体实施方式

[0078] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

[0079] 针对现有技术存在的问题，本发明提供了一种基于坐标系传递的SO(3)‑UKF姿态估计方法，下面结合附图对本发明作详细的描述。

[0080] 如图1所示，本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法包括：

[0081] 通过建立坐标系、定义姿态误差、进行sigma点采样、构造全局姿态所对应的sigma点、将姿态sigma点在姿态运动学方程中传递，构造传递后的姿态误差sigma点、进行UKF预测、更新得到姿态估计结果。

[0082] 如图2所示，本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法包括以下步骤：

[0083] S101，建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系

[0084] S102，进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

[0085] S103，将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

[0086] S104，进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

[0087] S105，进入下一时刻的滤波流程，重复步骤S101至步骤S104。

[0088] 本发明实施例提供的建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系 包括：

[0089] 建立姿态计算所在的坐标系即参考坐标系r，所述参考坐标系r可为惯性坐标系、地球坐标系或当地地理坐标系；

[0090] 建立载体坐标系b，所述载体坐标系b为惯性传感器即陀螺仪和加速度计所在的坐标系；

[0091] 建立估计参考坐标系 所述估计参考坐标系 为SO(3)‑UKF滤波算法中所估计的参考坐标系，所述估计参考坐标系 与所述参考坐标系r之间的误差即为姿态误差。

[0092] 本发明实施例提供的姿态误差定义如下：

[0093]

[0094] 其中， 表示真实的误差矩阵， 表示 对应的估计值。

[0095] 本发明实施例提供的进行sigma点采样并构造全局姿态所对应的sigma点包括：

[0096] 利用上一时刻姿态方差Pk‑1进行sigma点采样如下：

[0097]

[0098] 所述构造全局姿态所对应的sigma点包括：

[0099] 将姿态误差sigma点转换为矩阵形式：

[0100]

[0101] 其中，so3_exp[·]所定义的转换公式为：

[0102]

[0103] 利用上一时刻的姿态估计值 构造全局姿态所对应的sigma点

[0104]

[0105] 本发明实施例提供的将姿态sigma点在姿态运动学方程中传递包括：

[0106]

[0107] 其中，Ω(·)表示姿态运动学方程。

[0108] 本发明实施例提供的构造传递后的姿态误差sigma点；

[0109] 根据定义的姿态误差，构造传递后的姿态误差sigma点：

[0110]

[0111] 将矩阵形式的姿态误差转换为矢量形式的误差：

[0112]

[0113] 其中，so3_log[·]所定义的转换公式为：

[0114]

[0115] 本发明实施例提供的进行常规的UKF预测及预测方差计算如下：

[0116]

[0117]

[0118] 其中，w(i)表示sigma点所对应的权值，Qk‑1表示状态方差阵。

[0119] 本发明实施例提供的分别进行UKF量测更新以及全局姿态更新包括：

[0120] 所述进行UKF量测更新包括：

[0121] 通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即

[0122]

[0123] 其中，h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差；

[0124] 所述进行全局姿态更新包括：

[0125] 将矢量姿态误差估计 转换为矩阵形式

[0126]

[0127] 根据姿态误差定义，得到当前时刻的全局姿态更新:

[0128]

[0129] 本发明实施例提供的姿态误差估计置零如下：

[0130]

[0131] 本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：

[0132] 当姿态误差定义为 时，姿态sigma点构造为传递后的姿态误差sigma点为 姿态更新为

[0133] 当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

[0134] 当姿态误差定义为 时，姿态sigma点构造为 传递后的姿态误差sigma点为 姿态更新为

[0135] 本发明实施例提供的基于坐标系传递的SO(3)‑UKF姿态估计方法还包括：

[0136] 所述全局姿态可用四元数进行表示；所述姿态误差可用其它三维表示方法进行表示。

[0137] 本发明还提供一种基于坐标系传递的SO(3)‑UKF姿态估计系统，包括：

[0138] 坐标系构建模块，用于建立用于姿态计算的参考坐标系r、载体坐标系以及用于滤波的参考坐标系

[0139] 全局姿态对应的sigma点构建模块，用于进行姿态误差定义，进行sigma点采样并构造全局姿态所对应的sigma点；

[0140] 传递后的姿态误差sigma点构建模块，用于将姿态sigma点在姿态运动学方程中传递并构造传递后的姿态误差sigma点；

[0141] 更新模块，用于进行常规的UKF预测及预测方差计算，并分别进行UKF量测更新以及全局姿态更新；进行姿态误差估计置零；

[0142] 滤波模块，用于进入下一时刻的滤波流程。

[0143] 下面结合具体实施例对本发明的技术方案作进一步说明。

[0144] 实施例：

[0145] 本发明提供了四种不同的姿态误差定义，并给出了基于相应姿态误差定义所需要的sigma点采用、传递、更新以及全局姿态更新所涉及的矩阵传递方程，具体如表1所示。

[0146] 表1.不同姿态误差定义及其SO(3)‑UKF滤波算法设计中的应用

[0147]

[0148] 以参考系姿态误差定义(即表1中方案1)为例，本发明提供的一种基于坐标系传递的SO(3)‑UKF姿态估计设计方法具体步骤如下：

[0149] 步骤一、建立坐标系；

[0150] 定义参考坐标系为r，该坐标系为姿态解算所在的坐标系，根据实际应用可以为惯性坐标系、地球坐标系或当地地理坐标系等；

[0151] 定义载体坐标系为b，该坐标系为惯性传感器即陀螺仪和加速度计所在的坐标系(忽略安装误差)；

[0152] 定义估计参考坐标系为 该坐标系为SO(3)‑UKF滤波算法中所估计的参考坐标系，该坐标系与参考坐标系r之间的误差即为姿态误差。

[0153] 步骤二、定义姿态误差；

[0154] 记真实的误差矩阵为 其对应的估计值为 根据姿态矩阵链式相乘法则，姿态误差定义为

[0155]

[0156] 步骤三、进行sigma点采样；

[0157] 利用上一时刻姿态方差Pk‑1进行sigma点采样

[0158]

[0159] 步骤四、构造全局姿态所对应的sigma点；

[0160] 首先将(2)中姿态误差sigma点转换为矩阵形式

[0161]

[0162] 其中so3_exp[·]所定义的转换公式为

[0163]

[0164] 利用上一时刻的姿态估计值 构造全局姿态所对应的sigma点

[0165]

[0166] 步骤五、将姿态sigma点在姿态运动学方程中传递；

[0167]

[0168] 其中Ω(·)表示姿态运动学方程，为了简化起见，忽略了其中涉及的其它参数输入。

[0169] 步骤六、构造传递后的姿态误差sigma点；

[0170] 根据步骤一中所定义的姿态误差，构造传递后的姿态误差sigma点

[0171]

[0172] 将矩阵形式的姿态误差转换为矢量形式的误差

[0173]

[0174] 其中so3_log[·]所定义的转换公式为

[0175]

[0176] 步骤七、进行常规的UKF预测及预测方差计算；

[0177]

[0178]

[0179] 其中w(i)表示sigma点所对应的权值，Qk‑1为状态方差阵。

[0180] 步骤八、进行UKF量测更新；

[0181] 通过UKF量测更新得到矢量姿态误差估计及其对应的方差，即

[0182]

[0183] 其中h(·)表示量测方程，yk表示k时刻的量测信息，Rk表示量测噪声方差。同样，为了简化起见，此处忽略了量测方程h(·)中的参数输入。

[0184] 步骤九、全局姿态更新；

[0185] 将矢量姿态误差估计 转换为矩阵形式

[0186]

[0187] 根据步骤一中的姿态误差定义，得到当前时刻的全局姿态更新

[0188]

[0189] 步骤十、姿态误差估计置零。

[0190] 将姿态误差估计 置零，即

[0191]

[0192] 进而进入下一时刻的滤波流程，重复上述步骤。

[0193] 下面将本发明提供的方法应用至具体实施例中：

[0194] 以地球平面导航为例，载体航行轨迹如图2所示，依据具体的运动过程仿真产生相应的加速度和角速度信息并叠加相应的漂移和噪声误差作为惯性传感器输出，用于惯导解
算。以仿真轨迹附近的三个已知路标点作为观测信息。该仿真实例对应的状态方程和观测
方程分别为

[0195]

[0196]

[0197] 其中Ct，vt和pt分别表示载体的姿态矩阵、速度和位置。ωt表示由陀螺仪敏感的角速度信息， 是其对应的测量噪声。ut表示加速度计敏感的比力信息， 是其对应的测量噪声。g是重力矢量。(l1,l2,l3)是三个已知路标点。

[0198] 分别利用表1中方案1和方案3中的姿态误差定义参考本发明设计相应的SO(3)‑UKF算法，具体的轨迹跟踪结果如图3所示，位置和方位估计误差如图4和图5所示。从图中可以看出，本发明提供的方法可对运动载体的位置和姿态信息进行有效的跟踪，且精度由于
传统的EKF算法。

[0199] 在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对
本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。

[0200] 应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系
统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备
和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁
盘、CD或DVD‑ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模
集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软
件实现，也可以由上述硬件电路和软件的结合例如固件来实现。

[0201] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。