CN112729348B - 一种用于imu系统的姿态自适应校正方法 - Google Patents

Abstract

本发明提供一种用于IMU系统的姿态自适应校正方法，包括：获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点；将获取的采样点对应输入系统的状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；根据量测预测残差获取加速度计自适应校正因子和磁强计自适应校正因子判断自适应校正因子之间的大小对系统姿态进行自适应校正。本发明所述的用于IMU系统的姿态自适应校正方法具有计算效率高，精确等特点，可广泛应用于姿态参数估计领域。

Description

一种用于IMU系统的姿态自适应校正方法

技术领域

本发明涉及姿态参数估计领域，特别是涉及一种用于IMU(Inertial MeasurementUnit，惯性测量单元)系统的姿态自适应校正方法。

背景技术

传统的姿态校正方法是通过卡尔曼滤波的方法利用加速度计或磁强计对陀螺仪输出的系统姿态进行校正，适用于线性系统，但在煤矿井下定向钻进场合大多都为非线性系统，传统的方法并不适用。对于非线性系统，扩展卡尔曼滤波(EKF)则是对将非线性系统线性化，再进行卡尔曼滤波。但是在进行线性化的过程中忽略了其余高阶项，会引入较大误差，并且处理非线性较强的动态系统时，可能会导致滤波发散。无迹卡尔曼滤波的提出利用概率分布来逼近非线性，可达到EKF的二阶精度，被广泛的应用于航天和无人驾驶等姿态参数估计领域。然而在许多环境下，由于UKF需要的采样数据繁多，会导致算法效率较低。同时，在数值计算过程中状态误差协方差也可能会出现负定情况而无法抑制算法发散。

由此可见，在现有技术中，无迹卡尔曼滤波还存在计算效率低和算法发散等问题。

发明内容

有鉴于此，本发明的主要目的在于提供一种用于IMU系统的姿态自适应校正方法。

为了达到上述目的，本发明提出的技术方案为：

一种用于IMU系统的姿态自适应校正方法，包括如下步骤：

步骤1、根据IMU(Inertial Measurement Unit，惯性测量单元)系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程。

步骤2、根据UT(Unscented Transformation，无迹变换)形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数。

步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将第k时刻系统状态预测值、加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻。

步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差。

步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子和磁强计自适应校正因子/>其中，/>表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，/>表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔA^a为第k和第k-1时刻加速度计输出值差值，ΔH^h为第k和第k-1时刻磁强计输出值差值。

步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8。

步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。

步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。

综上所述，本发明所述一种用于IMU系统的姿态自适应校正方法首先获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；根据步骤2获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点，减少了算法的输入，提高了滤波效率；其次，对获取的采样点输入系统状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；再次，根据量测预测残差获取加速度计自适应校正因子 和磁强计自适应校正因子/>对系统姿态进行自适应校正，以此类推，不断自适应校正系统的姿态。因此，本发明所述顺序校正的超球体采样自适应无迹卡尔曼滤波方法在计算效率和精确度都有较大的提升。

附图说明

图1是本发明所述一种用于IMU(Inertial Measurement Unit，惯性测量单元)系统的姿态自适应校正方法的总体流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明作进一步地详细描述。

图1是本发明所述一种用于IMU(Inertial Measurement Unit，惯性测量单元)系统的姿态自适应校正方法的总体流程图。如图1所示，本发明所述用于IMU系统的姿态自适应校正方法，包括如下步骤：

步骤1、根据IMU系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程。

步骤2、根据UT(Unscented Transformation，无迹变换)形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数。

步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将第k时刻系统状态预测值、加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻。

步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差。

步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子和磁强计自适应校正因子/>其中，/>表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，/>表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔA^a为第k和第k-1时刻加速度计输出值差值，ΔH^h为第k和第k-1时刻磁强计输出值差值。

步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8。

步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。

步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。

本发明方法中，当加速度计和磁强计量测传感器任意量测具有不良数据而影响系统状态(姿态四元数)时，可以通过自适应因子来调节加速度计量测和磁强计量测的校正顺序，削弱不良数据的影响；当加速度计量测具有不良数据时，通过自适应顺序校正因子判断，使用加速度计量测对系统状态进行校正，加速度计对系统状态预测值和状态预测误差协方差对量测噪声协方差、状态四元数和状态估计协方差进行更新；然后使用磁强计对经过加速度计校正后的系统状态进行校正，更新加速度计校正后的系统状态估计值和状态估计协方差。当磁强计量测具有不良数据时，通过自适应顺序校正因子判断，使用磁强计量测对系统状态进行校正，磁强计对系统状态预测值和状态预测误差协方差对量测噪声协方差、状态四元数和状态估计协方差进行更新；然后使用加速度计对经过磁强计校正后的系统状态进行校正，更新磁强计校正后的系统状态估计值和状态估计协方差。

本发明步骤1中，所述系统状态方程、加速度计量测方程和磁强计量测方程具体为：

步骤a、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统的状态方程：

其中，表示第k时刻根据陀螺仪输出得到的姿态四元数向量，/>表示陀螺仪姿态四元数向量分量；这里，/>表示姿态参数中的旋转角度，/>分别表示姿态参数中x轴、y轴和z轴的轴向量分量；ΔT为陀螺仪数据的采样周期，I是4*4单位矩阵，/>为第k-1时刻陀螺仪输出的3*1角速度向量，/>是第k-1陀螺仪输出角速度组成的4*4矩阵；A_k-1为第k-1时刻的状态转移矩阵，且大小为4*4矩阵；W_k-1为第k-1时刻的系统噪声向量，且大小为4*1向量。

步骤b、根据加速度计输出加速度、磁强计输出磁场，建立系统加速度计量测方程和系统磁强计量测方程，分别如下：

其中，表示第k时刻加速度计量测值，/>表示第k时刻磁强计量测值，/>为第k时刻导航坐标系到载体坐标系的四元数转换矩阵，/>为第k时刻加速度计量测噪声，/>为第k时刻磁强计量测噪声，aⁿ为导航坐标系下重力加速度的参考值，hⁿ为导航坐标系下的磁场值，且hⁿ＝[cosβ，0，-sinβ]^T·B；这里，磁倾角β为地磁场与水平面之间的夹角，B为地磁场强度。

本发明方法步骤2中，所述UT形式采样具体为：

当i＝0时；

当i＝1，2，...，n时；

当i＝n+1，...，2n时；

其中，x_i表示第i个陀螺仪采样点状态，为系统状态，S表示系统状态误差协方差阵，n为系统状态向量维数，参数λ＝α²(n+K)-n；这里，参数α取值介于0～1之间，K为比例因子；按照UT形式采样，获取陀螺仪采样点向量x_i，i表示陀螺仪采样点序数，i＝0，1，2，...，2n+1，且i为自然数。

步骤2中，所述超球体单行采样规则具体步骤为：

步骤a、计算超球体单行采样初始点的均值权重：

其中，参数w_m取值均介于0～1之间，参数b为常数。

步骤b、计算其余n+1个超球体采样点的均值权重/>和协方差权重/>

其中，j表示加速度计或磁强计的采样点序数，j＝1，...，n+1，且j为自然数。

步骤c、超球体采样的n维迭代形式为：

步骤d、根据步骤c，从超球体采样的n维迭代形式，分别获取加速度计采样点状态向量磁强计采样点状态向量/>

本发明方法步骤3中，所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵具体为：

q_i，k＝A_k-1·x_i，k-1+W_k-1，其中i＝0，...2n；

其中i＝1，...2n；

其中i＝1，...2n；

其中，为第k时刻系统状态预测值，q_i，k为第k时刻的第i个系统状态，/>为第k时刻系统状态预测误差协方差阵，qr{}表示正交三角分解，cholupdate{}表示矩阵乔莱斯基分解，Q_k-1为第k-1时刻系统的噪声协方差阵，/>表示UT形式采样初始点均值权重，/>表示UT形式采样初始点协方差权重，/>表示UT形式其余采样点均值权重，/>表示UT形式其余采样点协方差权重。

本发明方法步骤4中，所述加速度计量测预测残差和磁强计量测预测残差具体为：

根据加速度计量测预测值和磁强计量测预测值/>按照加速度计在第k时刻输出值/>磁强计在第k时刻输出值/>其中，a_x，k，a_y，k，a_z，k分别对应加速度计x轴、y轴和z轴第k时刻的输出，h_x，k，h_y，k，h_z，k分别对应磁强计x轴、y轴和z轴第k时刻的输出；计算加速度计第k时刻量测预测残差和磁强计第k时刻量测预测残差为：

其中，表示第k时刻加速度计量测预测残差，/>表示第k时刻磁强计量测预测残差。

本发明方法步骤5中，所述加速度计自适应校正因子和磁强计自适应校正因子的计算，包括如下具体步骤：

步骤a、计算加速度计量测残差自适应校正因子和磁强计量测残差自适应校正因子/>其中，tr(·)为矩阵的迹，|·|表示绝对值符号。

步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔA^a的绝对值，得到加速度计自适应校正因子 在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔH^h的绝对值，得到磁强计自适应校正因子/>ΔA^a、ΔH^h分别如下：

其中，a_x，k-1，a_y，k-1，a_z，k-1分别表示加速度计x轴、y轴和z轴第k-1时刻的输出加速度分量，h_x，k-1，h_y，k-1，h_z，k-1分别表示磁强计x轴、y轴和z轴第k-1时刻的输出磁场分量，Δa_x，k，Δa_y，k，Δa_z，k为加速度计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值，Δh_x，k，Δh_y，k，Δh_z，k为磁强计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值。

本发明方法步骤6中，所述判断加速度计自适应校正因子和磁强计自适应校正因子的大小，执行姿态的自适应校正具体为：

当加速度计自适应校正因子大于磁强计自适应校正因子时，加速度计量测输出值和预测值之间的残差/>变大，加速度计的量测输出受到量测噪声的影响，执行步骤7。

当磁强计自适应校正因子大于加速度计自适应校正因子时：磁强计量测输出值和预测值之间的残差/>变大，磁强计的量测输出受到量测噪声的影响，执行步骤8。

本发明方法步骤7中，所述姿态校正方法，包括如下具体步骤：

步骤a、根据加速度计量测预测值和预测残差，对k时刻系统状态的预测值和k时刻系统状态的预测协方差阵/>进行校正，得到k时刻系统状态的估计值/>和k时刻系统状态的误差协方差阵/>

步骤b、根据磁强计的量测预测值和预测残差，对经过加速度计第校正后的系统状态估计值和状态误差协方差阵/>进行校正，得到k时刻系统状态的估计值/>和k时刻系统状态的误差协方差阵/>

本发明方法步骤8中，所述姿态校正方法，包括如下具体步骤：

步骤a、根据磁强计量测预测值和预测残差，对k时刻系统状态的预测值和k时刻系统状态的预测协方差阵/>进行校正，得到k时刻系统状态的估计值/>和k时刻系统状态的误差协方差阵/>

步骤b、根据加速度计的量测预测值和预测残差，对经过磁强计校正后的系统状态估计值和状态误差协方差阵/>进行校正，得到k时刻系统状态的估计值/>和k时刻系统状态的误差协方差阵/>

总之，本发明所述一种用于IMU系统的姿态自适应校正方法首先获取系统姿态的状态方程、加速度计量测方程和磁强计量测方程；获取系统状态方程的2n+1个采样点、加速度计和磁强计量测方程的n+2个采样点，减少了算法的输入，提高了滤波效率；其次，对获取的采样点输入系统状态方程，加速度计量测方程和磁强计量测方程，进行状态预测；根据状态预测值获取量测预测残差；再次，根据量测预测残差获取加速度计自适应校正因子和磁强计自适应校正因子/>对系统姿态进行自适应校正，以此类推，不断自适应校正系统的姿态。因此，本发明所述顺序校正的超球体采样自适应无迹卡尔曼滤波方法在计算效率和精确度都有较大的提升。

实际应用中，由于本发明所述一种用于IMU系统的姿态自适应校正方法不仅在获取系统采样点、加速度计量测和磁强计量测采样点时考虑到算法的计算效率性，还在校正过程中考虑了量测残差，进一步的提高了算法的收敛性原则，故本发明所述用于IMU系统的姿态自适应校正方法的计算效率和精确度都得到了进一步提升。

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

Claims (6)

1.一种用于IMU系统的姿态自适应校正方法，其特征在于，所述校正方法包括如下步骤：

步骤1、根据IMU系统陀螺仪输出角速度计算陀螺仪姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统状态方程；根据IMU系统加速度计输出加速度和IMU系统磁强计输出磁场，分别建立系统加速度计量测方程和系统磁强计量测方程；

步骤2、根据UT形式对系统状态方程采样2n+1个点，对系统加速度计量测方程按照超球体单行采样规则采样n+2个点，对系统磁强计量测方程按照超球体单行采样规则采样n+2个点，其中，n为系统状态方程中系统状态向量的维数，且n为自然数；

步骤3、将步骤2获取的2n+1个采样点输入系统状态方程，对系统状态进行预测，得到第k时刻系统状态预测值，并获取第k时刻系统状态预测误差协方差；同时，将加速度计n+2个采样点、磁强计n+2个采样点，对应输入系统加速度计量测方程、系统磁强计量测方程，得到第k时刻加速度计量测预测值和第k时刻磁强计量测预测值；其中，第k时刻为当前时刻；

步骤4、根据第k时刻加速度计量测预测值、第k时刻磁强计量测预测值和加速度计输出值、磁强计输出值，获取加速度计量测预测残差和磁强计量测预测残差；

步骤5、根据加速度计量测预测残差和磁强计量测预测残差，获取加速度计自适应校正因子和磁强计自适应校正因子/>其中，/>表示从第k-1时刻到第k时刻加速度计的量测残差自适应校正因子，/>表示从第k-1时刻到第k时刻磁强计的量测残差自适应校正因子，ΔA^a为第k和第k-1时刻加速度计输出值差值，ΔH^h为第k和第k-1时刻磁强计输出值差值；

步骤6、判断步骤5得到的加速度计自适应校正因子与磁强计自适应校正因子之间的大小：当加速度计自适应校正因子大于磁强计自适应校正因子时，则执行步骤7；当磁强计自适应校正因子大于加速度计自适应校正因子时，则执行步骤8；

步骤7、根据系统第k时刻加速度计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻磁强计量测预测值和预测残差，对经过加速度计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正；

步骤8、根据系统第k时刻磁强计量测预测值和预测残差，对第k时刻系统状态预测值和系统状态预测误差协方差阵进行校正，得到第k时刻系统状态估计值和第k时刻系统状态误差协方差阵；之后，采用系统第k时刻加速度计量测预测值和预测残差，对经过磁强计校正后得到的第k时刻系统状态估计值和第k时刻系统状态误差协方差阵进行校正。

2.根据权利要求1所述的IMU系统的姿态自适应校正方法，其特征在于，步骤1包括如下具体步骤：

步骤a、根据系统的陀螺仪输出角速度计算陀螺仪的姿态四元数，将得到的陀螺仪姿态四元数作为系统状态，建立系统的状态方程：

其中，表示第k时刻根据陀螺仪输出得到的姿态四元数向量，表示陀螺仪姿态四元数向量分量；这里，/>表示姿态参数中的旋转角度，/>分别表示姿态参数中x轴、y轴和z轴的轴向量分量；ΔT为陀螺仪数据的采样周期，I是4*4单位矩阵，/>为第k-1时刻陀螺仪输出的3*1角速度向量，/>是第k-1陀螺仪输出角速度组成的4*4矩阵，A_k-1为第k-1时刻的状态转移矩阵，且大小为4*4矩阵；W_k-1为第k-1时刻的系统噪声向量，且大小为4*1向量；

步骤b、根据加速度计输出加速度、磁强计输出磁场，建立系统加速度计量测方程和系统磁强计量测方程，分别如下：

其中，表示第k时刻加速度计量测值，/>表示第k时刻磁强计量测值，/>为第k时刻导航坐标系到载体坐标系的四元数转换矩阵，/>为第k时刻加速度计量测噪声，/>为第k时刻磁强计量测噪声，aⁿ为导航坐标系下重力加速度的参考值，hⁿ为导航坐标系下的磁场值，且hⁿ＝[cosβ，0，-sinβ]^T·B；这里，磁倾角β为地磁场与水平面之间的夹角，B为地磁场强度。

3.根据权利要求1或2所述的IMU系统的姿态自适应校正方法，其特征在于，步骤2中，所述UT形式采样为：

当i＝0时；

当i＝1，2，...，n时；

当i＝n+1，...，2n时；

其中，x_i表示第i个陀螺仪采样点状态，为系统状态，S表示系统状态误差协方差阵，n为系统状态向量维数，参数λ＝α²(n+K)-n；这里，参数α取值介于0～1之间，K为比例因子；按照UT形式采样，获取陀螺仪采样点状态向量的集合{x_i}，i表示陀螺仪采样点序数，i＝0，1，2，...，2n，且i为自然数；

步骤2中，所述的超球体单行采样规则，包括如下具体步骤：

步骤a、计算超球体单行采样初始点的均值权重：

其中，参数w_m取值均介于0～1之间，参数b为常数；

步骤b、计算其余n+1个超球体采样点中j的取值是从1到n+1，即得到n+1个超球体采样点；

均值权重和协方差权重/>

步骤c、超球体采样的n维迭代形式为：

步骤d、根据步骤c，从超球体采样的n维迭代形式，分别获取n+2个加速度计采样点状态向量n+2个磁强计采样点状态向量/>

其中，j表示加速度计或磁强计的采样点序数，j＝1，...，n+1，且j为自然数。

4.根据权利要求3所述的IMU系统的姿态自适应校正方法，其特征在于，步骤3中，所述第k时刻系统状态预测值和第k时刻系统状态预测误差协方差阵，分别如下：

q_i，k＝A_k-1·x_i，k-1+W_k-1，其中i＝0，...2n

其中i＝1，...2n

其中i＝1，...2n

其中，为第k时刻系统状态预测值，q_i，k为第k时刻第i个系统状态，/>为第k时刻系统状态预测误差协方差阵，qr{}表示正交三角分解，cholupdate{}表示矩阵乔莱斯基分解，Q_k-1为第k-1时刻系统的噪声协方差阵，/>表示UT形式采样初始点均值权重，/>表示UT形式采样初始点协方差权重，/>表示UT形式其余采样点均值权重，/>表示UT形式其余采样点协方差权重；

步骤3中、所述加速度计的量测预测值和磁强计的量测预测值，是将超球体采样得到的加速度计采样点状态向量和磁强计采样点状态向量/>分别输入加速度计量测方程和磁强计量测方程进行预测，获得第k时刻加速度计量测预测值/>第k时刻磁强计量测预测值/>

当j＝0，1，...n+1时；

其中，

5.根据权利要求4所述的IMU系统的姿态自适应校正方法，其特征在于，步骤4中，所述加速度计量测预测残差和磁强计量测预测残差具体为：

根据加速度计量测预测值和磁强计量测预测值/>按照加速度计在第k时刻输出值磁强计在第k时刻输出值/>其中，a_x，k，a_y，k，a_z，k分别对应加速度计x轴、y轴和z轴第k时刻的输出，h_x，k，h_y，k，h_z，k分别对应磁强计x轴、y轴和z轴第k时刻的输出；计算加速度计第k时刻量测预测残差和磁强计第k时刻量测预测残差为：

其中，表示第k时刻加速度计量测预测残差，/>表示第k时刻磁强计量测预测残差。

6.根据权利要求5所述的IMU系统的姿态自适应校正方法，其特征在于，步骤5中，所述加速度计自适应校正因子和磁强计自适应校正因子的计算，包括如下具体步骤：

步骤a、计算加速度计量测残差自适应校正因子和磁强计量测残差自适应校正因子/>其中，tr(·)为矩阵的迹，|·|表示绝对值符号；

步骤b、在加速度计量测残差自适应校正因子前乘上第k和第k-1两个时刻加速度计输出值差值ΔA^a的绝对值，得到加速度计自适应校正因子 在磁强计量测残差自适应校正因子前乘上第k和第k-1两个时刻磁强计输出值差值ΔH^h的绝对值，得到磁强计自适应校正因子/>ΔA^a、ΔH^h分别如下：

其中，a_x，k-1，a_y，k-1，a_z，k-1分别表示加速度计x轴、y轴和z轴第k-1时刻的输出加速度分量，h_x，k-1，h_y，k-1，h_z，k-1分别表示磁强计x轴、y轴和z轴第k-1时刻的输出磁场分量，Δa_x，k，Δa_y，k，Δa_z，k为加速度计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值，Δh_x，k，Δh_y，k，Δh_z，k为磁强计对应的x轴、y轴和z轴第k时刻和第k-1时刻的输出差值。