CN105606096B - 一种载体运动状态信息辅助的姿态和航向计算方法和*** - Google Patents

一种载体运动状态信息辅助的姿态和航向计算方法和*** Download PDF

Info

Publication number
CN105606096B
CN105606096B CN201610059857.XA CN201610059857A CN105606096B CN 105606096 B CN105606096 B CN 105606096B CN 201610059857 A CN201610059857 A CN 201610059857A CN 105606096 B CN105606096 B CN 105606096B
Authority
CN
China
Prior art keywords
mrow
msub
mover
mtd
msup
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201610059857.XA
Other languages
English (en)
Other versions
CN105606096A (zh
Inventor
高楠
赵龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN201610059857.XA priority Critical patent/CN105606096B/zh
Publication of CN105606096A publication Critical patent/CN105606096A/zh
Application granted granted Critical
Publication of CN105606096B publication Critical patent/CN105606096B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Acyclic And Carbocyclic Compounds In Medicinal Compositions (AREA)

Abstract

本发明公开一种载体运动状态信息辅助的姿态和航向计算方法和***，该方法包括以下步骤：根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息；根据载体的运动状态信息，判断是否存在环境磁干扰；当存在环境磁干扰时，根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵；根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。本发明根据载体的运动状态信息检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性。

Description

一种载体运动状态信息辅助的姿态和航向计算方法和***
技术领域
本发明涉及通信技术领域，特别涉及一种载体运动状态信息辅助的姿态和航向计算方法和***。
背景技术
姿态和航向参考***(Attitude and Heading Reference System，简称AHRS)是一种可以精确测量载体在空间坐标系下三轴姿态的测量装置，由三轴磁强计、三轴陀螺仪和三轴加速度计等硬件和相应的软件构成，为载体提供实时的姿态和航向信息，在航空航天、机器人、汽车工业以及行人导航定位领域得到广泛的应用。随着微机电技术的发展，基于微机电***传感器的姿态和航向参考***应用更广泛。但由于陀螺仪存在漂移，导致在计算载体姿态和航向信息，其误差不断累积。为解决此问题，在姿态和航向参考***中引入加速度计和磁传感器的信息，并以重力场和地磁场作为参考矢量，利用加速计和磁强计输出值与陀螺仪的输出值实时计算载体的姿态和航向信息。
虽然姿态和航向参考***可为载体实时提供精确的姿态和航向信息，但在近地运动的载体经常受到外部环境的干扰，特别是磁传感器受环境磁干扰明显。当受到外部磁干扰时，利用磁传感器输出值作为量测值估计载体的姿态和航向信息存在误差，甚至不能正确计算载体的姿态和航向信息，从而给载体的自主控制带来很大的风险。
发明内容
本发明提供了一种载体运动状态信息辅助的姿态和航向计算方法和***，以解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。
本发明提供了一种载体运动状态信息辅助的姿态和航向计算方法，包括以下步骤：
根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息；
根据所述载体的运动状态信息，判断是否存在环境磁干扰；
当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算所述载体姿态信息和航向信息。
可选地，根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算所述载体姿态信息和航向信息，具体包括：
根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更新值更新姿态四元数的估计值；
根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；
根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
可选地，根据所述加速度计量测值更新状态预测值，得到状态更新值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；
根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
可选地，根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；
根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
可选地，所述Kalman滤波的状态方程，具体为：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
可选地，所述第一量测方程，具体为：
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
可选地，根据所述Kalman滤波的状态方程、所述第一量测方程式以及所述加速度计量测值更新状态预测值，得到状态更新值，具体为：
采用以下公式更新状态预测值，得到状态更新值：
其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka，k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
可选地，使用所述状态更新值更新姿态四元数的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
可选地，所述第二量测方程，具体为：
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
可选地，根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：
采用以下公式更新状态更新值，得到状态估计值：
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
可选地，使用所述状态估计值更新所述姿态四元数的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
可选地，根据加速度计的输出值和陀螺仪的输出值，获取所述载体的运动状态信息，具体为：
判断所述加速度计的输出值是否满足第一条件：
其中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
如果所述加速度计的输出值满足第一条件，则确定所述载体处于静止状态；否则，判断所述陀螺仪的输出值是否满足第二条件：
其中，为陀螺仪的输出值，θω为第二阈值；
如果所述陀螺仪的输出值满足第二条件，则确定所述载体处于直行状态；否则，确定所述载体处于转弯状态。
可选地，根据所述载体的运动状态信息，判断是否存在环境磁干扰，具体为：
判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值，如果是，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
可选地，判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值之前，还包括：
采用以下公式构建所述卡方假设检验统计量：
其中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，m为被检测观测维数。
可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，具体为：
采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重之前，还包括：
采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
可选地，根据所述载体的运动状态信息，判断是否存在环境磁干扰之后，还包括：
当存在环境磁干扰时，根据所述载体的运动状态信息，使用运动状态中的健康残差自适应取代受干扰后的残差。
可选地，使用运动状态中的健康残差自适应取代受干扰后的残差之前，还包括：
采用以下公式计算运动状态中的健康残差：
其中，为运动状态中的健康残差，[r1，r2，…rn]为所述载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
本发明还提供了一种姿态和航向参考***，包括：
获取模块，用于根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息；
判断模块，用于根据所述载体的运动状态信息，判断是否存在环境磁干扰；
第一构建模块，用于在所述判断模块判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重；
第二构建模块，用于根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
第一计算模块，用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
可选地，所述第一计算模块，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
可选地，所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
可选地，所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
可选地，所述Kalman滤波的状态方程，具体为：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
可选地，所述第一量测方程，具体为：
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
可选地，所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预测值，得到状态更新值：
其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka，k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
可选地，所述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
可选地，所述第二量测方程，具体为：
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
可选地，所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程，并采用以下公式更新状态更新值，得到状态估计值：
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
可选地，所述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
可选地，所述获取模块，具体用于判断所述加速度计的输出值是否满足第一条件：
其中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
如果所述加速度计的输出值满足第一条件，则确定所述载体处于静止状态；否则，判断所述陀螺仪的输出值是否满足第二条件：
其中，为陀螺仪的输出值，θω为第二阈值；
如果所述陀螺仪的输出值满足第二条件，则确定所述载体处于直行状态；否则，确定所述载体处于转弯状态。
可选地，所述判断模块，具体用于判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值，如果是，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
可选地，所述的***，还包括：
第三构建模块，用于采用以下公式构建所述卡方假设检验统计量：
其中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，m为被检测观测维数。
可选地，所述第一构建模块，具体用于在所述判断模块判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
可选地，所述的***，还包括：
第二计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
可选地，所述的***，还包括：
取代模块，用于当存在环境磁干扰时，根据所述载体的运动状态信息，使用运动状态中的健康残差自适应取代受干扰后的残差。
可选地，所述的***，还包括：
第三计算模块，用于采用以下公式计算运动状态中的健康残差：
其中，为运动状态中的健康残差，[r1，r2，…rn]为所述载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
本发明根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息，进而根据载体的运动状态信息检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。
附图说明
图1为本发明实施例中的一种载体运动状态信息辅助的姿态和航向计算方法流程图；
图2为本发明实施例中的另一种载体运动状态信息辅助的姿态和航向计算方法流程图；
图3为本发明实施例中的一种姿态和航行参考***的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供了一种载体运动状态信息辅助的姿态和航向计算方法，如图1所示，包括以下步骤：
步骤101，根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息。
步骤102，根据载体的运动状态信息，判断是否存在环境磁干扰。
步骤103，当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建磁传感器量测值的自适应权重，并根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵。
步骤104，根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
本发明实施例根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息，进而根据载体的运动状态信息检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。
本发明实施例还提供了另一种载体运动状态信息辅助的姿态和航向计算方法，如图2所示，包括以下步骤：
步骤201，根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息。
其中，载体的运动状态信息可以是静止状态，也可以是直行状态，还可以是转弯状态。
具体地，可以根据加速度计的输出值，采用公式(1)检测载体是否处于静止状态：
式中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
本实施例中，取时间窗为1.5s，取λ2＝0.02m/s2。
进一步地，根据陀螺仪的输出值，采用公式(2)检测载体处于直行状态或转弯状态：
式中，为陀螺仪的输出值，θω为第二阈值；
本实施例中，取θω＝0.01rad/s。
当式(1)成立时，载体处于静止状态；反之，式(1)不成立时，载体处于非静止状态；当式(2)成立时，载体处于直行状态；反之，式(2)不成立时，载体处于转弯状态。
步骤202，根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程。
具体地，可以取误差四元数的矢量部分qe、陀螺仪随机常值bω和加速度计偏差ba作为状态量，构建Kalman滤波的状态方程：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量；T为转置；F(t)为状态转移矩阵；W(t)为过程噪声；为陀螺仪输出值的反对矩阵；vω为陀螺仪的随机漂移噪声；为陀螺仪随机常值模型噪声；为加速度计随机常值模型噪声；
步骤203，根据地球重力场参考向量和地磁场参考向量在载体坐标系的投影值分别与加速度计和磁传感器实测值之差建立量测方程。
具体地，利用加速度计的输出值与加速度计输出值的估计值的差La以及磁传感器输出值和磁传感器输出值的估计值的差Lm分别构建第一量测方程和第二量测方程：
其中，为加速度计的输出值；为磁传感器的输出值；为从导航坐标系n到载体坐标系b的转化矩阵；为四元数的估计值；gn＝[0 0 -g]T为重力加速度g在导航坐标系n下的投影；mn＝[0 cosα sinα]T为理想地磁场输出值的归一化结果；α为磁倾角；va和vm分别为加速度计和磁力计的随机误差。
本实施例中，gn＝9.8m/s2。
步骤204，采用两步观测更新的Kalman滤波算法估计四元数误差和惯性传感器误差，通过姿态四元数更新方程实现载体的姿态和航向估计。
具体地，根据式(6)，利用在k时刻的观测值La，k更新k时刻的状态预测值
其中，为量测方程式(6)在k时刻的量测矩阵；I为单位阵；为在k时刻状态预测值的更新值；Ka，k为增益矩阵；为在k时刻状态预测值的协方差矩阵；Ra为加速度计量测噪声的协方差矩阵；为在k时刻状态的协方差矩阵；
本实施例中，Ra＝(500μg)2I。
利用状态量中的分量更新
其中，符号为四元数乘法；为四元数的归一化；为赋值符号，将等式右侧的计算值赋给等式左侧的变量；
根据式(7)，利用在k时刻的量测值Lm，k更新k时刻的获得k时刻的状态估计值和状态估计值的协方差矩阵
其中，Km，k为k时刻的增益矩阵；Lm，k为k时刻的由磁传感器计算的量测值；Am，k为k时刻的量测矩阵； 为k时刻磁传感器量测值的等价权矩阵；
根据式(11)和式(12)利用状态量中的分量来更新并利用来计算载体的姿态和航向信息。
步骤205，根据Kalman滤波的预测残差构建卡方假设检验统计量，利用载体的运动状态信息生成不同运动状态下的阈值，对环境磁干扰进行检测。
具体地，可以根据Kalman滤波的预测残差，采用公式(17)构建卡方假设检验统计量：
式中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，mm为被检测观测维数；
进一步地，根据卡方假设检验统计量，采用公式(17)构建环境磁干扰检测准则：
λk＞TD (18)
式中，TD为与载体的运动状态信息对应的阈值，即，载体处于静止状态、直行状态或转弯状态时，可以分别对应不同的TD。
本实施例中，载体处于静止状态和直行状态时，TD的取值为7.88；载体处于转弯状态时，TD的取值为15。
当式(18)成立时，存在环境磁干扰；当式(18)不成立时，不存在环境磁干扰。
步骤206，根据环境磁干扰的检测结果，利用等价权矩阵在线调整磁传感器量测值的权重，使计算出的载体姿态和航向信息具有抗差性。
根据量测方程式(7)计算磁传感器观测值的归一化残差为
其中，Lmi为量测方程式(7)的第i个测量方程；Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根；
本实施例中，Rm＝0.001I；
根据环境磁干扰检测准则和磁传感器量测值的归一化残差构建量测值权重因子为
式中，为在k时刻磁传感器第i个量测值的自适应权重；pki为给定量测值协方差矩阵的第i个主对角线元素；c为常数；
本实施例中，c＝1.3-2.0。
此外，根据载体的运动状态信息，采用公式(21)使用运动状态中的健康残差自适应取代受干扰后的残差：
式中，为运动状态中的健康残差，[r1，r2，…rn]为载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
本发明实施例利用k时刻磁传感器量测值的自适应权重i＝1，2，3，构建磁传感器量测值的等价权矩阵并实现磁传感器量测值对状态估计值贡献的自适应调整，使姿态和航向信息计算具有抗差性。
本发明的优点在于：将载体的运动状态信息引入到载体的姿态和航向信息计算中，根据自适应两步Kalman滤波器预测残差构建卡方假设检验统计量，利用所述车辆运动状态感知信息生成不同运动状态下的阈值，对滤波算法中的地磁场量测是否存在干扰进行检测；根据磁干扰检测结果，利用等价权矩阵降低磁干扰量测值的权重，当载体处于环境磁干扰状态时，利用相同运动状态的健康残差自适应调整干扰的残差，使姿态与航向信息对于磁场畸变具有抗差性，为载体提供具有鲁棒性的姿态和航向信息，为实现载体自主运动的导航和控制奠定了基础。
基于上述载体运动状态信息辅助的姿态和航向计算方法，本发明实施例还提供了一种姿态和航行参考***，如图3所示，包括：
获取模块310，用于根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息。
判断模块320，用于根据载体的运动状态信息，判断是否存在环境磁干扰；
第一构建模块330，用于在判断模块320判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建磁传感器量测值的自适应权重；
第二构建模块340，用于根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵；
第一计算模块350，用于根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
其中，第一计算模块350，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
具体地，上述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
上述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
其中，Kalman滤波的状态方程，具体为：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
其中，第一量测方程，具体为：
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
相应地，上述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预测值，得到状态更新值：
其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka，k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
上述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
其中，上述第二量测方程，具体为：
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
相应地，上述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程，并采用以下公式更新状态更新值，得到状态估计值：
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
上述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
上述获取模块，具体用于判断加速度计的输出值是否满足第一条件：
其中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
如果加速度计的输出值满足第一条件，则确定载体处于静止状态；否则，判断陀螺仪的输出值是否满足第二条件：
其中，为陀螺仪的输出值，θω为第二阈值；
如果陀螺仪的输出值满足第二条件，则确定载体处于直行状态；否则，确定载体处于转弯状态。
上述判断模块320，具体用于判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值，如果是，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
相应地，上述***，还包括：
第三构建模块，用于采用以下公式构建卡方假设检验统计量：
其中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，m为被检测观测维数。
上述第一构建模块330，具体用于在判断模块320判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
进一步地，上述***，还包括：
第二计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Kmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
进一步地，上述***，还包括：
取代模块，用于当存在环境磁干扰时，根据载体的运动状态信息，使用运动状态中的健康残差自适应取代受干扰后的残差。
相应地，上述***，还包括：
第三计算模块，用于采用以下公式计算运动状态中的健康残差：
其中，为运动状态中的健康残差，[r1，r2，…rn]为所述载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
本发明的优点在于：将载体的运动状态信息引入到载体的姿态和航向信息计算中，根据自适应两步Kalman滤波器预测残差构建卡方假设检验统计量，利用所述车辆运动状态感知信息生成不同运动状态下的阈值，对滤波算法中的地磁场量测是否存在干扰进行检测；根据磁干扰检测结果，利用等价权矩阵降低磁干扰量测值的权重，当载体处于环境磁干扰状态时，利用相同运动状态的健康残差自适应调整干扰的残差，使姿态与航向信息对于磁场畸变具有抗差性，为载体提供具有鲁棒性的姿态和航向信息，为实现载体自主运动的导航和控制奠定了基础。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法和***，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如：多个单元或组件可以结合，或可以集成到另一个***，或一些特征可以忽略，或不执行。此外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。
上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。
此外，在本发明各实施例中的各功能单元可以全部集成在一个处理模块中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
本发明适用于载体姿态和航向参考***信息计算。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (36)

1.一种载体运动状态信息辅助的姿态和航向计算方法，其特征在于，包括以下步骤：
根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息；
根据所述载体的运动状态信息，判断是否存在环境磁干扰；
当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算所述载体姿态信息和航向信息。
2.如权利要求1所述的方法，其特征在于，根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算所述载体姿态信息和航向信息，具体包括：
根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更新值更新姿态四元数的估计值；
根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；
根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
3.如权利要求2所述的方法，其特征在于，根据所述加速度计量测值更新状态预测值，得到状态更新值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；
根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
4.如权利要求2所述的方法，其特征在于，根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；
根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
5.如权利要求3或4所述的方法，其特征在于，所述Kalman滤波的状态方程，具体为：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mo>[</mo> <msup> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> </mtd> <mtd> <mo>-</mo> <mn>0.5</mn> <mi>I</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mn>0.5</mn> <msub> <mi>v</mi> <mi>&amp;omega;</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>b</mi> <mi>&amp;omega;</mi> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>b</mi> <mi>a</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
6.如权利要求3所述的方法，其特征在于，所述第一量测方程，具体为：
<mrow> <msub> <mi>L</mi> <mi>a</mi> </msub> <mo>=</mo> <msup> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>=</mo> <mn>2</mn> <mo>[</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>b</mi> <mi>a</mi> </msub> <mo>+</mo> <msub> <mi>v</mi> <mi>a</mi> </msub> </mrow>
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
7.如权利要求6所述的方法，其特征在于，根据所述Kalman滤波的状态方程、所述第一量测方程式以及所述加速度计量测值更新状态预测值，得到状态更新值，具体为：
采用以下公式更新状态预测值，得到状态更新值：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msubsup> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>R</mi> <mi>a</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>R</mi> <mi>a</mi> </msub> <msubsup> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow>
其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka，k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
8.如权利要求2所述的方法，其特征在于，使用所述状态更新值更新姿态四元数的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CircleTimes;</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> </mrow>
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>/</mo> <mo>|</mo> <mo>|</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>|</mo> <mo>|</mo> </mrow>
<mrow> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
9.如权利要求4所述的方法，其特征在于，所述第二量测方程，具体为：
<mrow> <msub> <mi>L</mi> <mi>m</mi> </msub> <mo>=</mo> <msup> <mover> <mi>m</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>m</mi> <mi>n</mi> </msup> <mo>=</mo> <mn>2</mn> <mo>[</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>v</mi> <mi>m</mi> </msub> </mrow>
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
10.如权利要求9所述的方法，其特征在于，根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：
采用以下公式更新状态更新值，得到状态估计值：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>r</mi> <mn>3</mn> </msub> <msubsup> <mi>r</mi> <mn>3</mn> <mi>T</mi> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msubsup> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msup> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open='[' close=']' separators=''> <mtable> <mtr> <mtd> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3,1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>r</mi> <mn>3</mn> </msub> <mo>=</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
11.如权利要求2所述的方法，其特征在于，使用所述状态估计值更新所述姿态四元数的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CircleTimes;</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> </mrow>
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>/</mo> <mo>|</mo> <mo>|</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>|</mo> <mo>|</mo> </mrow>
<mrow> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
12.如权利要求1所述的方法，其特征在于，根据加速度计的输出值和陀螺仪的输出值，获取所述载体的运动状态信息，具体为：
判断所述加速度计的输出值是否满足第一条件：
<mfenced open='{' close=''> <mtable> <mtr> <mtd> <mo>|</mo> <mi>Max</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>X</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mi>Min</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>X</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>|</mo> <mo>&amp;le;</mo> <msub> <mi>&amp;lambda;</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mo>|</mo> <mi>Max</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>Y</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mi>Min</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>Y</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>|</mo> <mo>&amp;le;</mo> <msub> <mi>&amp;lambda;</mi> <mn>2</mn> </msub> </mtd> </mtr> </mtable> </mfenced>
其中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
如果所述加速度计的输出值满足第一条件，则确定所述载体处于静止状态；否则，判断所述陀螺仪的输出值是否满足第二条件：
<mrow> <msubsup> <mi>&amp;omega;</mi> <mi>z</mi> <mi>b</mi> </msubsup> <mo>&amp;le;</mo> <msub> <mi>&amp;theta;</mi> <mi>&amp;omega;</mi> </msub> </mrow>
其中，为陀螺仪的输出值，θω为第二阈值；
如果所述陀螺仪的输出值满足第二条件，则确定所述载体处于直行状态；否则，确定所述载体处于转弯状态。
13.如权利要求1所述的方法，其特征在于，根据所述载体的运动状态信息，判断是否存在环境磁干扰，具体为：
判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值，如果是，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
14.如权利要求13所述的方法，其特征在于，判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值之前，还包括：
采用以下公式构建所述卡方假设检验统计量：
<mrow> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <msub> <mover> <mi>V</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <msub> <mi>&amp;Sigma;</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> <msub> <mover> <mi>V</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <mo>~</mo> <msup> <mi>&amp;chi;</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </mrow>
其中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，m为被检测观测维数。
15.如权利要求1所述的方法，其特征在于，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，具体为：
采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
16.如权利要求9所述的方法，其特征在于，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重之前，还包括：
采用以下公式计算所述磁传感器量测值的归一化残差：
<mrow> <msup> <msub> <mi>V</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>mi</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>mi</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mo>|</mo> <msub> <mi>&amp;sigma;</mi> <mi>i</mi> </msub> <mo>|</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1,2,3</mn> </mrow>
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
17.如权利要求1所述的方法，其特征在于，根据所述载体的运动状态信息，判断是否存在环境磁干扰之后，还包括：
当存在环境磁干扰时，根据所述载体的运动状态信息，使用运动状态中的健康残差自适应取代受干扰后的残差。
18.如权利要求17所述的方法，其特征在于，使用运动状态中的健康残差自适应取代受干扰后的残差之前，还包括：
采用以下公式计算运动状态中的健康残差：
<mrow> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msup> <mi>r</mi> <mn>1</mn> </msup> <mo>+</mo> <msup> <mi>r</mi> <mn>2</mn> </msup> <mo>+</mo> <mo>&amp;CenterDot;</mo> <mo>&amp;CenterDot;</mo> <mo>&amp;CenterDot;</mo> <msup> <mi>r</mi> <mi>n</mi> </msup> </mrow> <mi>n</mi> </mfrac> </mrow>
其中，为运动状态中的健康残差，[r1，r2，…rn]为所述载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
19.一种姿态和航向参考***，其特征在于，包括：
获取模块，用于根据加速度计的输出值和陀螺仪的输出值，获取载体的运动状态信息；
判断模块，用于根据所述载体的运动状态信息，判断是否存在环境磁干扰；
第一构建模块，用于在所述判断模块判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重；
第二构建模块，用于根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
第一计算模块，用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
20.如权利要求19所述的***，其特征在于，所述第一计算模块，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
21.如权利要求20所述的***，其特征在于，
所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
22.如权利要求20所述的***，其特征在于，
所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
23.如权利要求21或22所述的***，其特征在于，所述Kalman滤波的状态方程，具体为：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mo>[</mo> <msup> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> </mtd> <mtd> <mo>-</mo> <mn>0.5</mn> <mi>I</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mn>0.5</mn> <msub> <mi>v</mi> <mi>&amp;omega;</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>b</mi> <mi>&amp;omega;</mi> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <msub> <mi>b</mi> <mi>a</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
24.如权利要求21所述的***，其特征在于，所述第一量测方程，具体为：
<mrow> <msub> <mi>L</mi> <mi>a</mi> </msub> <mo>=</mo> <msup> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>=</mo> <mn>2</mn> <mo>[</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>b</mi> <mi>a</mi> </msub> <mo>+</mo> <msub> <mi>v</mi> <mi>a</mi> </msub> </mrow>
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
25.如权利要求24所述的***，其特征在于，
所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预测值，得到状态更新值：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msubsup> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>R</mi> <mi>a</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>R</mi> <mi>a</mi> </msub> <msubsup> <mi>K</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow>
其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka，k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
26.如权利要求20所述的***，其特征在于，
所述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CircleTimes;</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> </mrow>
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>/</mo> <mo>|</mo> <mo>|</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>|</mo> <mo>|</mo> </mrow>
<mrow> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
27.如权利要求22所述的***，其特征在于，所述第二量测方程，具体为：
<mrow> <msub> <mi>L</mi> <mi>m</mi> </msub> <mo>=</mo> <msup> <mover> <mi>m</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>m</mi> <mi>n</mi> </msup> <mo>=</mo> <mn>2</mn> <mo>[</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>]</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>v</mi> <mi>m</mi> </msub> </mrow>
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
28.如权利要求27所述的***，其特征在于，
所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程，并采用以下公式更新状态更新值，得到状态估计值：
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>r</mi> <mn>3</mn> </msub> <msubsup> <mi>r</mi> <mn>3</mn> <mi>T</mi> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msubsup> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <msub> <mi>A</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msup> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>m</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open='[' close=']' separators=''> <mtable> <mtr> <mtd> <msub> <mi>&amp;Sigma;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3,1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <msub> <mn>0</mn> <mrow> <mn>6</mn> <mo>&amp;times;</mo> <mn>6</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>r</mi> <mn>3</mn> </msub> <mo>=</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
29.如权利要求20所述的***，其特征在于，
所述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CircleTimes;</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> </mrow>
<mrow> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;DoubleLeftArrow;</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>/</mo> <mo>|</mo> <mo>|</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>|</mo> <mo>|</mo> </mrow>
<mrow> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>:</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
30.如权利要求19所述的***，其特征在于，
所述获取模块，具体用于判断所述加速度计的输出值是否满足第一条件：
<mfenced open='{' close=''> <mtable> <mtr> <mtd> <mo>|</mo> <mi>Max</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>X</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mi>Min</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>X</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>|</mo> <mo>&amp;le;</mo> <msub> <mi>&amp;lambda;</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mo>|</mo> <mi>Max</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>Y</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mi>Min</mi> <mrow> <mo>(</mo> <mi>STD</mi> <mrow> <mo>(</mo> <mi>Accel</mi> <mo>.</mo> <msup> <mi>Y</mi> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>|</mo> <mo>&amp;le;</mo> <msub> <mi>&amp;lambda;</mi> <mn>2</mn> </msub> </mtd> </mtr> </mtable> </mfenced>
其中，λ2为第一阈值，Accel.Xb为时间窗内x轴加速度计的输出值，Accel.Yb为时间窗内y轴加速度计的输出值，Max为最大值，Min为最小值，STD为标准差，上角标b为载体坐标系；
如果所述加速度计的输出值满足第一条件，则确定所述载体处于静止状态；否则，判断所述陀螺仪的输出值是否满足第二条件：
<mrow> <msubsup> <mi>&amp;omega;</mi> <mi>z</mi> <mi>b</mi> </msubsup> <mo>&amp;le;</mo> <msub> <mi>&amp;theta;</mi> <mi>&amp;omega;</mi> </msub> </mrow>
其中，为陀螺仪的输出值，θω为第二阈值；
如果所述陀螺仪的输出值满足第二条件，则确定所述载体处于直行状态；否则，确定所述载体处于转弯状态。
31.如权利要求19所述的***，其特征在于，
所述判断模块，具体用于判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值，如果是，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
32.如权利要求31所述的***，其特征在于，还包括：
第三构建模块，用于采用以下公式构建所述卡方假设检验统计量：
<mrow> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <msub> <mover> <mi>V</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <msub> <mi>&amp;Sigma;</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> <msub> <mover> <mi>V</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <mo>~</mo> <msup> <mi>&amp;chi;</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </mrow>
其中，和分别为Kalman滤波的预测残差和预测残差协方差矩阵，m为被检测观测维数。
33.如权利要求19所述的***，其特征在于，
所述第一构建模块，具体用于在所述判断模块判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
34.如权利要求27所述的***，其特征在于，还包括：
第二计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
<mrow> <msup> <msub> <mi>V</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mrow> <mi>mi</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>A</mi> <mrow> <mi>mi</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mo>|</mo> <msub> <mi>&amp;sigma;</mi> <mi>i</mi> </msub> <mo>|</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1,2,3</mn> </mrow>
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
35.如权利要求19所述的***，其特征在于，还包括：
取代模块，用于当存在环境磁干扰时，根据所述载体的运动状态信息，使用运动状态中的健康残差自适应取代受干扰后的残差。
36.如权利要求35所述的***，其特征在于，还包括：
第三计算模块，用于采用以下公式计算运动状态中的健康残差：
<mrow> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msup> <mi>r</mi> <mn>1</mn> </msup> <mo>+</mo> <msup> <mi>r</mi> <mn>2</mn> </msup> <mo>+</mo> <mo>&amp;CenterDot;</mo> <mo>&amp;CenterDot;</mo> <mo>&amp;CenterDot;</mo> <msup> <mi>r</mi> <mi>n</mi> </msup> </mrow> <mi>n</mi> </mfrac> </mrow>
其中，为运动状态中的健康残差，[r1，r2，…rn]为所述载体处于静止状态和直行状态下利用滑动时间窗保存的健康残差。
CN201610059857.XA 2016-01-28 2016-01-28 一种载体运动状态信息辅助的姿态和航向计算方法和*** Active CN105606096B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610059857.XA CN105606096B (zh) 2016-01-28 2016-01-28 一种载体运动状态信息辅助的姿态和航向计算方法和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610059857.XA CN105606096B (zh) 2016-01-28 2016-01-28 一种载体运动状态信息辅助的姿态和航向计算方法和***

Publications (2)

Publication Number Publication Date
CN105606096A CN105606096A (zh) 2016-05-25
CN105606096B true CN105606096B (zh) 2018-03-30

Family

ID=55986228

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610059857.XA Active CN105606096B (zh) 2016-01-28 2016-01-28 一种载体运动状态信息辅助的姿态和航向计算方法和***

Country Status (1)

Country Link
CN (1) CN105606096B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106443745A (zh) * 2016-10-27 2017-02-22 广州日滨科技发展有限公司 一种航向角校正的方法及装置
CN106885566B (zh) * 2017-01-03 2019-12-17 浙江大学 一种可穿戴式运动传感器及其抗磁场干扰的方法
CN106500695B (zh) * 2017-01-05 2019-02-01 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN109211226A (zh) * 2017-06-29 2019-01-15 沈阳新松机器人自动化股份有限公司 一种基于mems运动传感器二维姿态解算的方法及装置
FR3069633B1 (fr) * 2017-07-28 2019-08-23 Sysnav Determination de cap a partir du champ mesure par des capteurs magnetiques
CN109459020B (zh) * 2018-12-24 2022-06-21 哈尔滨工程大学 一种惯性和磁力计组合自适应抗干扰方法
CN110045152A (zh) * 2019-05-14 2019-07-23 爱动超越人工智能科技（北京）有限责任公司 一种特种车辆行进状态检测方法及装置
CN111811502B (zh) * 2020-07-10 2022-07-22 北京航空航天大学 一种运动载体多源信息融合导航方法及***
CN112013836B (zh) * 2020-08-14 2022-02-08 北京航空航天大学 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN114279396A (zh) * 2021-12-22 2022-04-05 Oppo广东移动通信有限公司 运动姿态确定方法及装置、计算机可读介质和电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考***解算精度和***抗干扰能力的方法
CN103776451A (zh) * 2014-03-04 2014-05-07 哈尔滨工业大学 一种基于mems的高精度三维姿态惯性测量***以及测量方法
CN103822633A (zh) * 2014-02-11 2014-05-28 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN104296745A (zh) * 2014-09-29 2015-01-21 杭州电子科技大学 一种基于9-dof传感器组的姿态检测数据融合方法
CN104567871A (zh) * 2015-01-12 2015-04-29 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考***解算精度和***抗干扰能力的方法
CN103822633A (zh) * 2014-02-11 2014-05-28 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN103776451A (zh) * 2014-03-04 2014-05-07 哈尔滨工业大学 一种基于mems的高精度三维姿态惯性测量***以及测量方法
CN104296745A (zh) * 2014-09-29 2015-01-21 杭州电子科技大学 一种基于9-dof传感器组的姿态检测数据融合方法
CN104567871A (zh) * 2015-01-12 2015-04-29 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

Also Published As

Publication number Publication date
CN105606096A (zh) 2016-05-25

Similar Documents

Publication Publication Date Title
CN105606096B (zh) 一种载体运动状态信息辅助的姿态和航向计算方法和***
CN105716610B (zh) 一种地磁场模型辅助的载体姿态和航向计算方法和***
CN103017763B (zh) 状态估计设备和偏移更新方法
CN109163721A (zh) 姿态测量方法及终端设备
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及***
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN108225308A (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN103389506B (zh) 一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法
CN109855617A (zh) 一种车辆定位方法、车辆定位装置及终端设备
CN107525503A (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
JP2010032398A (ja) ナビゲーションシステムの位置検出装置および位置検出方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
Sabet et al. Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
Zhao et al. Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
AU2015305864A2 (en) Earthmoving machine comprising weighted state estimator
CN110375731A (zh) 一种混合交互式多模型滤波方法
Keighobadi et al. Design and experimental evaluation of block-pulse functions and Legendre polynomials observer for attitude-heading reference system
CN107860382A (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
Maliňák et al. Pure-inertial AHRS with adaptive elimination of non-gravitational vehicle acceleration
EP3255382B1 (en) Heading inconsistency mitigation for inertial navigation using low-performance inertial measurement units with relative aiding
Ercan et al. Multi-sensor data fusion of DCM based orientation estimation for land vehicles

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant