CN105716610B - 一种地磁场模型辅助的载体姿态和航向计算方法和*** - Google Patents

一种地磁场模型辅助的载体姿态和航向计算方法和*** Download PDF

Info

Publication number
CN105716610B
CN105716610B CN201610059817.5A CN201610059817A CN105716610B CN 105716610 B CN105716610 B CN 105716610B CN 201610059817 A CN201610059817 A CN 201610059817A CN 105716610 B CN105716610 B CN 105716610B
Authority
CN
China
Prior art keywords
value
magnetic
magnetic sensor
state
estimation
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
CN201610059817.5A
Other languages
English (en)
Other versions
CN105716610A (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 CN201610059817.5A priority Critical patent/CN105716610B/zh
Publication of CN105716610A publication Critical patent/CN105716610A/zh
Application granted granted Critical
Publication of CN105716610B publication Critical patent/CN105716610B/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/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)

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为所述状态估计值的分量，为所述状态估计值。
可选地，根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角，具体为：
采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体系输出值的方向矢量，nm为所述磁传感器在载体系输出值的方向矢量。
可选地，根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰，具体为：
判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD
其中，Dreference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值；
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，具体为：
采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重之前，还包括：
采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
本发明还提供了一种姿态和航向参考***，包括：
第一计算模块，用于根据载体的位置信息和地磁场模型，计算理想磁倾角；
第二计算模块，用于根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；
判断模块，用于根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；
第一构建模块，用于在所述判断模块判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重；
第二构建模块，用于根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
第三计算模块，用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
可选地，所述第三计算模块，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
可选地，所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建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为所述状态估计值的分量，为所述状态估计值。
可选地，所述第二计算模块，具体用于采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体系输出值的方向矢量，nm为所述磁传感器在载体系输出值的方向矢量。
可选地，所述判断模块，具体用于判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD
其中，Dreference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值；
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
可选地，所述第一构建模块，具体用于在所述判断模块判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
可选地，所述的***，还包括：
第四计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
本发明根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。
附图说明
图1为本发明实施例中的一种地磁场模型辅助的载体姿态和航向计算方法流程图；
图2为本发明实施例中的另一种地磁场模型辅助的载体姿态和航向计算方法流程图；
图3为本发明实施例中的一种姿态和航行参考***的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供了一种地磁场模型辅助的载体姿态和航向计算方法，如图1所示，包括以下步骤：
步骤101，根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角。
步骤102，根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰。
步骤103，当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建磁传感器量测值的自适应权重，并根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵。
步骤104，根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
本发明实施例根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。
本发明实施例还提供了另一种地磁场模型辅助的载体姿态和航向计算方法，如图2所示，包括以下步骤：
步骤201，根据载体的位置信息和地磁场模型，计算理想地磁场的总磁场强度、磁倾角和磁偏角，将理想地磁场的磁倾角作为理想磁倾角。
具体地，可以根据美国国家地球物理信息中心(NGDC)和英国地质测量局(BGS)制作，并由美国国家地理空间情报局(NGA)发布的世界地磁模型(World Magnetic Model，简称WMM)以及载体运动的轨迹数据，实时计算载体所在位置处的地磁场信息，该地磁场信息包括总磁场强度、磁偏角和磁倾角。
步骤202，根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角。
具体地，可以采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm) (1)
其中，Dcacul为估计磁倾角，na和nm分别为加速度计和磁传感器在载体系输出值的方向矢量。
步骤203，根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰。
具体地，可以判断理想磁倾角和估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD (2)
其中，Dreference为理想磁倾角，Dcacul为估计磁倾角，λD为预设阈值。
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
本实施例中，λD＝0.55°。
步骤204，根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程。
具体地，可以取误差四元数的矢量部分qe、陀螺仪随机常值bω和加速度计偏差ba作为状态量，构建Kalman滤波的状态方程：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量；T为转置；F(t)为状态转移矩阵；W(t)为过程噪声；为陀螺仪输出值的反对矩阵；vω为陀螺仪的随机漂移噪声；为陀螺仪随机常值模型噪声；为加速度计随机常值模型噪声；
步骤205，根据地球重力场参考向量和地磁场参考向量在载体坐标系的投影值分别与加速度计和磁传感器实测值之差建立量测方程。
具体地，利用加速度计的输出值与加速度计输出值的估计值的差La以及磁传感器输出值和磁传感器输出值的估计值的差Lm分别构建第一量测方程和第二量测方程：
其中，为加速度计的输出值；为磁传感器的输出值；为从导航坐标系n到载体坐标系b的转化矩阵；为四元数的估计值；gn＝[0 0 -g]T为重力加速度g在导航坐标系n下的投影；mn＝[0 cosα sinα]T为理想地磁场输出值的归一化结果；α为磁倾角；va和vm分别为加速度计和磁力计的随机误差。
本实施例中，gn＝9.8m/s2。
步骤206，采用两步观测更新的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)利用状态量中的分量来更新并利用来计算载体的姿态和航向信息。
步骤207，根据步骤203中的环境磁干扰的检测结果，利用等价权矩阵在线调整磁传感器量测值的权重，使计算出的载体姿态和航向信息具有抗差性。
根据量测方程式(7)计算磁传感器观测值的归一化残差为
其中，Lmi为量测方程式(7)的第i个测量方程；Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根；
本实施例中，Rm＝0.001I；
根据环境磁干扰检测准则和磁传感器量测值的归一化残差构建量测值权重因子为
式中，为在k时刻磁传感器第i个量测值的自适应权重；pki为给定量测值协方差矩阵的第i个主对角线元素；c为常数；
本实施例中，c＝1.3-2.0。
本发明实施例利用k时刻磁传感器量测值的自适应权重构建磁传感器量测值的等价权矩阵并实现磁传感器量测值对状态估计值贡献的自适应调整，使姿态和航向信息计算具有抗差性。
本发明的优点在于：将姿态和航向参考***信息计算的过程采用两步滤波的方法来实现，第一步以加速度计为量测传感器来估计四元数的误差并更新四元数；第二步以磁传感器为量测传感器估计四元数的误差，并利用磁干扰检测算法实时检测载体所处环境的磁干扰，通过量测残差和实际残差的比值自适应调节磁传感器观测数据在滤波估计中的权重，更新四元数并计算载体的姿态和航向参考***信息。提高了载体姿态和航向参考***信息的抗差性和可靠性。
基于上述地磁场模型辅助的载体姿态和航向计算方法，本发明实施例还提供了一种姿态和航行参考***，如图3所示，包括：
第一计算模块310，用于根据载体的位置信息和地磁场模型，计算理想磁倾角；
第二计算模块320，用于根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；
判断模块330，用于根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰；
第一构建模块340，用于在判断模块330判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建磁传感器量测值的自适应权重；
第二构建模块350，用于根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵；
第三计算模块360，用于根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
其中，第三计算模块360，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息。
具体地，上述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建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为所述状态估计值的分量，为所述状态估计值。
上述第二计算模块320，具体用于采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体系输出值的方向矢量，nm为所述磁传感器在载体系输出值的方向矢量。
上述判断模块330，具体用于判断理想磁倾角和估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD
其中，Dreference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值；
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
上述第一构建模块340，具体用于在判断模块330判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
进一步地，上述***，还包括：
第四计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
本发明的优点在于：将姿态和航向参考***信息计算的过程采用两步滤波的方法来实现，第一步以加速度计为量测传感器来估计四元数的误差并更新四元数；第二步以磁传感器为量测传感器估计四元数的误差，并利用磁干扰检测算法实时检测载体所处环境的磁干扰，通过量测残差和实际残差的比值自适应调节磁传感器观测数据在滤波估计中的权重，更新四元数并计算载体的姿态和航向参考***信息。提高了载体姿态和航向参考***信息的抗差性和可靠性。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法和***，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如：多个单元或组件可以结合，或可以集成到另一个***，或一些特征可以忽略，或不执行。此外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。
上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。
此外，在本发明各实施例中的各功能单元可以全部集成在一个处理模块中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
本发明适用于载体姿态和航向参考***信息计算。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (22)

1.一种地磁场模型辅助的载体姿态和航向计算方法，其特征在于，包括以下步骤：
根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；
根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；
当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
其中，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi,k′为磁传感器量测值的归一化残差；
根据加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息，具体包括以下步骤：
(1)根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更新值更新姿态四元数的估计值；
(2)根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；
(3)根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息；
其中步骤(2)又具体包括：根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；
根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：
采用以下公式更新状态更新值，得到状态估计值：
为k时刻的状态估计值，为k时刻的状态更新值，Lm,k为k时刻的所述磁传感器量测值，为的协方差矩阵，为k时刻的协方差矩阵，T为转置，为从导航坐标系到载体坐标系的转化矩阵，Km,k为k时刻的增益矩阵，I为单位阵,Am,k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵，为所述姿态四元数的估计值。
2.如权利要求1所述的方法，其特征在于，根据所述加速度计量测值更新状态预测值，得到状态更新值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；
根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
3.如权利要求2所述的方法，其特征在于，所述Kalman滤波的状态方程，具体为：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对称矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
4.如权利要求2所述的方法，其特征在于，所述第一量测方程，具体为：
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
5.如权利要求4所述的方法，其特征在于，根据所述Kalman滤波的状态方程、所述第一量测方程式以及所述加速度计量测值更新状态预测值，得到状态更新值，具体为：
其中，为k时刻的状态更新值，为k时刻的状态预测值，La,k为k时刻的所述加速度计量测值，Aa,k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka,k为k时刻的增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
6.如权利要求1所述的方法，其特征在于，使用所述状态更新值更新姿态四元数的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为所述状态更新值。
7.如权利要求1所述的方法，其特征在于，所述第二量测方程，具体为：
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场计算值的归一化结果，vm为磁传感器的随机误差。
8.如权利要求1所述的方法，其特征在于，使用所述状态估计值更新所述姿态四元数的估计值，具体为：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
9.如权利要求1所述的方法，其特征在于，根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角，具体为：
采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体坐标系输出值的方向矢量，nm为所述磁传感器在载体坐标系输出值的方向矢量。
10.如权利要求1所述的方法，其特征在于，根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰，具体为：
判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD
其中，Dreference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值；
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
11.如权利要求7所述的方法，其特征在于，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重之前，还包括：
采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi,k为k时刻所述第二量测方程的第i个量测值，Ami,k为k时刻量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
12.一种姿态和航向参考***，其特征在于，包括：
第一计算模块，用于根据载体的位置信息和地磁场模型，计算理想磁倾角；
第二计算模块，用于根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；
判断模块，用于根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；
第一构建模块，用于在所述判断模块判断出存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重；
所述第一构建模块，具体用于在所述判断模块判断出存在环境磁干扰时，采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的第i个主对角线元素，c为常数，Vi,k′为磁传感器量测值的归一化残差；
第二构建模块，用于根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；
第三计算模块，用于根据加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息；
所述第三计算模块，包括：
第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新值；
第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；
第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值；
第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；
计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息；
所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；
根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：
并采用以下公式更新状态更新值，得到状态估计值：
其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm,k为k时刻的所述磁传感器量测值，为的协方差矩阵，Km,k为k时刻的增益矩阵，Am,k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵。
13.如权利要求12所述的***，其特征在于，
所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。
14.如权利要求13所述的***，其特征在于，所述Kalman滤波的状态方程，具体为：
其中，X(t)＝[qe bω ba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程噪声，为陀螺仪输出值的反对称矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪随机常值，ba为加速度计偏差。
15.如权利要求13所述的***，其特征在于，所述第一量测方程，具体为：
其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
16.如权利要求15所述的***，其特征在于，
所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预测值，得到状态更新值：
其中，为k时刻的状态更新值，为k时刻的状态预测值，La,k为k时刻的所述加速度计量测值，Aa,k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，Ka,k为增益矩阵，为的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
17.如权利要求12所述的***，其特征在于，
所述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态更新值的分量，为k时刻的状态更新值。
18.如权利要求12所述的***，其特征在于，所述第二量测方程，具体为：
其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm为磁传感器的随机误差。
19.如权利要求13所述的***，其特征在于，
所述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：
其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，qe为所述状态估计值的分量，为所述状态估计值。
20.如权利要求12所述的***，其特征在于，
所述第二计算模块，具体用于采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体坐标系输出值的方向矢量，nm为所述磁传感器在载体坐标系输出值的方向矢量。
21.如权利要求12所述的***，其特征在于，
所述判断模块，具体用于判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：
|Dcacul-Dreference|＞λD
其中，Dreference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值。
22.如权利要求18所述的***，其特征在于，还包括：
第四计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：
其中，Lmi,k为k时刻所述第二量测方程的第i个量测值，Ami,k为k时刻量测矩阵的第i行；|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
CN201610059817.5A 2016-01-28 2016-01-28 一种地磁场模型辅助的载体姿态和航向计算方法和*** Active CN105716610B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610059817.5A CN105716610B (zh) 2016-01-28 2016-01-28 一种地磁场模型辅助的载体姿态和航向计算方法和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610059817.5A CN105716610B (zh) 2016-01-28 2016-01-28 一种地磁场模型辅助的载体姿态和航向计算方法和***

Publications (2)

Publication Number Publication Date
CN105716610A CN105716610A (zh) 2016-06-29
CN105716610B true CN105716610B (zh) 2018-09-04

Family

ID=56154242

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610059817.5A Active CN105716610B (zh) 2016-01-28 2016-01-28 一种地磁场模型辅助的载体姿态和航向计算方法和***

Country Status (1)

Country Link
CN (1) CN105716610B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105937911A (zh) * 2016-07-01 2016-09-14 南京理工大学 一种磁传感器姿态解算方法
CN108955669A (zh) * 2017-05-17 2018-12-07 田亮 一种重磁场组合导航算法
CN107421535B (zh) * 2017-05-22 2021-11-26 上海交通大学 一种基于磁场特征和加速度信息计步的室内行人定位***
CN107860382B (zh) * 2017-11-07 2021-04-06 吉林大学 一种在地磁异常情况下应用ahrs测量姿态的方法
CN108844536B (zh) * 2018-04-04 2020-07-03 中国科学院国家空间科学中心 一种基于量测噪声协方差矩阵估计的地磁导航方法
CN109115241A (zh) * 2018-08-07 2019-01-01 中国航空无线电电子研究所 飞机航向数据源完好性监控方法
CN111649763B (zh) * 2020-04-26 2021-08-27 中国人民解放军61540部队 一种基于重力灯塔建立的潜艇导航方法及***
CN112050804B (zh) * 2020-07-31 2022-04-08 东南大学 一种基于地磁梯度的近场磁图构建方法
CN112013836B (zh) * 2020-08-14 2022-02-08 北京航空航天大学 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN112066984B (zh) * 2020-09-17 2022-08-30 深圳维特智能科技有限公司 姿态角度解算方法、装置、处理设备和存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿***惯性/地磁组合方法
CN103528587B (zh) * 2013-10-15 2016-09-28 西北工业大学 自主组合导航***
CN103822633B (zh) * 2014-02-11 2016-12-07 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法

Also Published As

Publication number Publication date
CN105716610A (zh) 2016-06-29

Similar Documents

Publication Publication Date Title
CN105716610B (zh) 一种地磁场模型辅助的载体姿态和航向计算方法和***
CN105606096B (zh) 一种载体运动状态信息辅助的姿态和航向计算方法和***
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN109163721A (zh) 姿态测量方法及终端设备
Shin et al. An unscented Kalman filter for in-motion alignment of low-cost IMUs
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及***
US9417091B2 (en) System and method for determining and correcting field sensors errors
US8756995B2 (en) Device and method for combining samples from an inertial measurement sensor cluster
CN108225308A (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN110231029B (zh) 一种水下机器人多传感器融合数据处理方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN109000640A (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109916395A (zh) 一种姿态自主冗余组合导航算法
CN110017837A (zh) 一种姿态抗磁干扰的组合导航方法
CN109073388B (zh) 旋磁地理定位***
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
Sabet et al. Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS
CN106979781A (zh) 基于分布式惯性网络的高精度传递对准方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
Bistrov Performance analysis of alignment process of MEMS IMU
CN116147624B (zh) 一种基于低成本mems航姿参考***的船舶运动姿态解算方法
CN110375731A (zh) 一种混合交互式多模型滤波方法
Sokolović et al. INS/GPS navigation system based on MEMS technologies
Wang et al. Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm

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