[发明专利]基于坐标系传递的姿态估计方法、系统、终端、介质及应用

权利要求书

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点包括：

利用上一时刻姿态方差P_k-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点所对应的权值，Q_k-1表示状态方差阵；

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

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

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

其中，h(·)表示量测方程，y_k表示k时刻的量测信息，R_k表示量测噪声方差；

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

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

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

所述姿态误差估计置零如下：