CN108871319B - 一种基于地球重力场与地磁场序贯修正的姿态解算方法 - Google Patents

一种基于地球重力场与地磁场序贯修正的姿态解算方法 Download PDF

Info

Publication number
CN108871319B
CN108871319B CN201810384477.2A CN201810384477A CN108871319B CN 108871319 B CN108871319 B CN 108871319B CN 201810384477 A CN201810384477 A CN 201810384477A CN 108871319 B CN108871319 B CN 108871319B
Authority
CN
China
Prior art keywords
carrier
attitude
matrix
coordinate system
quaternion
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.)
Expired - Fee Related
Application number
CN201810384477.2A
Other languages
English (en)
Other versions
CN108871319A (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.)
Individual
Original Assignee
Individual
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 Individual filed Critical Individual
Priority to CN201810384477.2A priority Critical patent/CN108871319B/zh
Publication of CN108871319A publication Critical patent/CN108871319A/zh
Application granted granted Critical
Publication of CN108871319B publication Critical patent/CN108871319B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种基于地球重力场与地磁场序贯修正的姿态解算方法，其步骤包括如下：(1)根据航姿系统中陀螺仪采集的角速率计算系统模型的状态转移矩阵，并利用离散化的四元数微分方程更新载体姿态四元数；(2)根据航姿系统中加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正；(3)根据航姿系统中磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。本发明利用地球物理场信息对更新得到的姿态四元数进行了两次序贯最优估计，在该算法中，不需向系统提供先验地磁场信息，只需提供先验重力加速度矢量，克服了上述地磁场在应用中的不足。

Description

一种基于地球重力场与地磁场序贯修正的姿态解算方法
技术领域
本发明属于定位导航技术领域，具体涉及一种基于地球重力场与地磁场序贯修正的姿态解算方法。
技术背景
航姿参考系统(Attitude and Heading Reference System，AHRS)中集成了MEMS传感器(陀螺仪、加速度计、磁力计)，其有成本低、集成度高以及体积小等优点，但是在进行姿态解算时，却存在输出噪声过大、零点漂移无法完全消除、角速率输出易受干扰等所导致的姿态解算精度低、累计误差大等问题。因此，通常情况下，需要采用多个传感器经过信息融合算法实现高精度的姿态解算。相较于利用欧拉角进行姿态解算，四元数作为一种非常有用的数学工具，由于其参数少且避免了欧拉角产生的奇异，在现代姿态解算应用中，得到了广泛的应用。地磁场作为地球固有的物理特征，在导航中的应用越来越广泛，但是地磁场易受周围建筑物及钢性结构的影响，在地球表面的不同位置，地磁场矢量各不相同且未知，大大限制的地磁场在导航中的应用。
发明内容
为解决以上技术问题本发明提供一种基于地球重力场与地磁场序贯修正的姿态解算方法。
本发明的技术方案是：一种基于地球重力场与地磁场序贯修正的姿态解算方法，包括如下三个步骤：
(1)根据航姿系统(AHRS)中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵，并利用离散化的四元数微分方程更新载体姿态四元数；
(2)根据航姿系统(AHRS)中MEMS加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正；
(3)根据航姿系统(AHRS)中MEMS磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。
详细地，步骤(1)具体包括：
a.建立陀螺仪动力学模型：
Figure BDA0001641914320000011
即
Figure BDA0001641914320000021
其中，X＝[X0 X1 X2 X3]T表示全局坐标系G到载体坐标系B的旋转四元数，
Figure BDA0001641914320000022
表示在载体坐标系中测量的MEMS陀螺角速度信息，且
Figure BDA0001641914320000023
利用四元数微分方程的比卡求解法，使上式离散化可得
Figure BDA0001641914320000024
其中，Xk-1为tk-1时刻全局坐标系G到载体坐标系B的旋转四元数，Xk为tk时刻全局坐标系G到载体坐标系B的旋转四元数，
Figure BDA0001641914320000025
且
Figure BDA0001641914320000026
b.获得载体坐标系B下的载体角速度信息
Figure BDA0001641914320000027
c.根据所述载体角速度信息计算系统状态转移矩阵
Figure BDA0001641914320000028
c.载体姿态四元数状态一步预测
Figure BDA0001641914320000029
Figure BDA00016419143200000210
d.计算系统噪声协方差矩阵Qk。
对于MEMS陀螺仪在微小时间间隔Δt，其真实的角增量Δθ°是无法测量的，假设在tk-1时刻到tk时刻的时间间隔内，陀螺仪的实际输出与真实角增量的误差为δθk/k-1，有
Figure BDA00016419143200000211
其中，Δθk/k-1表示陀螺仪的实际角增量，
Figure BDA00016419143200000212
表示陀螺仪的真实角增量。
相应的，实际状态转移矩阵与真实状态转移矩阵的误差为ΔΦk/k-1，则有
Figure BDA0001641914320000031
将上式代入步骤a中陀螺仪离散化的动力学模型中，可得
Figure BDA0001641914320000039
Figure BDA0001641914320000032
变形为
-δΘk/k-1Xk-1＝-Ξ(Xk-1)δθk/k-1
式中
Figure BDA0001641914320000033
设陀螺仪的输出噪声为ngyro,则有
Figure BDA0001641914320000034
则系统噪声协方差矩阵为：
Figure BDA0001641914320000035
其中，Σgyro为陀螺仪噪声协方差矩阵。
f.计算姿态四元数状态一步预测均方误差Pk/k-1。
Figure BDA0001641914320000036
其中，Pk-1表示步骤(3)中的状态估计均方误差。
详细地，步骤(2)具体包括：
a.建立加速度计观测模型：
将三维矢量g与a看作零标量的四元数，则g与a间的变换关系可用四元数乘法表示为
Figure BDA0001641914320000037
其中，g为在全局坐标系G中测量的重力加速度，a为在载体坐标系B中测量的重力加速度，且满足||g||＝||a||＝1；
Figure BDA0001641914320000038
表示姿态四元数Xk的逆。
上式经过变换可得
M′(a)Xk-M(g)Xk＝0
其中，M′(a)与M(g)表示由零标量的四元数g与a构成的矩阵，如下
Figure BDA0001641914320000041
Figure BDA0001641914320000042
b.获得载体坐标系B下的载体重力加速度信息a＝[ax ay az]T；
c.根据所述载体重力加速度信息及先验重力加速度计算量测矩阵
Figure BDA0001641914320000043
d.计算重力加速度量测噪声协方差矩阵Rg′k
设加速度计的输出噪声为nacc，则有
nacc＝a-a°
其中，a°表示载体坐标系下加速度计的真实值。
实际量测矩阵与理想量测矩阵误差为
Figure BDA0001641914320000044
其中，
Figure BDA0001641914320000045
表示理想量测矩阵。
将上式代入步骤a中的加速度计观测模型，可得
0＝HgkXk-ΔHgkXk
-ΔHgkXk变形为
Figure BDA0001641914320000046
其中，
Figure BDA0001641914320000051
令
Figure BDA0001641914320000052
则量测噪声协方差矩阵为
Figure BDA0001641914320000053
其中，Σacc为加速度计噪声协方差矩阵。
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
Rg′k＝Rgk+βgI βg>0
e.计算重力加速度修正的滤波增益Kgk
Kgk＝Pk/k-1(Hgk)T(HgkPk/k-1(Hgk)T+Rg′k)-1
f.基于重力加速度的姿态四元数状态
Figure BDA0001641914320000054
估计。
Figure BDA0001641914320000055
g.基于重力加速度的姿态四元数状态估计均方误差Pgk。
Pgk＝(I-KgkHgk)Pk/k-1
详细地，步骤(3)具体包括：
a.建立磁力计观测模型：
把三维矢量h与m看作零标量的四元数，则h与m间的变换关系可用四元数乘法表示为
Figure BDA0001641914320000056
其中，h为在全局坐标系G中测量的地磁场矢量，m为在载体坐标系B中测量的地磁场矢量，且满足||g||＝||a||＝1。
上式经过变换可得
M′(m)Xk-M(h)Xk＝0
其中，M′(m)与M(h)表示由零标量的四元数h与m构成的矩阵，如下
Figure BDA0001641914320000061
Figure BDA0001641914320000062
b.获得载体坐标系B下的载体地磁场矢量信息m＝[mx my mz]T；
c.计算先验地磁场矢量h。
全局参考坐标系G的Gz轴选取重力加速度g的反方向，Gx轴选取地磁北极，Gy按照右手法则选取，则先验地磁场矢量h与全局参考坐标系G的GxoGz面重合即h＝[hx 0 hy]T，则有
Figure BDA0001641914320000063
其中，l＝[lx ly lz]T表示带有误差的先验地磁场矢量。
则有
Figure BDA0001641914320000064
式中
Γ＝lx 2+ly 2
d.根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵
Figure BDA0001641914320000065
e.计算地磁场矢量量测噪声协方差矩阵Rm′k。
设磁力计的输出噪声为nmag，则有
nmag＝m-m°
其中，m°表示载体坐标系下加速度计的真实值。
实际量测矩阵与理想量测矩阵误差为
Figure BDA0001641914320000071
其中，
Figure BDA0001641914320000072
表示理想量测矩阵。
把上式代入步骤a中的磁力计观测模型，可得
0＝HmkXk-ΔHmkXk
-ΔHmkXk变形为
Figure BDA0001641914320000073
其中，
Figure BDA0001641914320000074
令
Figure BDA0001641914320000075
则量测噪声协方差矩阵为
Figure BDA0001641914320000076
其中，Σmag为磁力计噪声协方差矩阵。
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
Rm′k＝Rmk+βmI βm>0
f.计算地磁场矢量修正的滤波增益Kmk。
Kmk＝Pgk(Hmk)T(HmkPgk(Hmk)T+Rm′k)-1
g.基于地磁场矢量的姿态四元数状态
Figure BDA0001641914320000077
估计。
Figure BDA0001641914320000078
h.基于地磁场矢量的姿态四元数状态估计均方误差Pmk。
Pmk＝(I-KmkHmk)Pgk
其中，I表示单位矩阵。
Figure BDA0001641914320000079
本发明的优点是：该算法利用地球物理场(地球重力场、地磁场)信息对更新得到的姿态四元数进行了两次序贯最优估计。在该算法中，不需向系统提供先验地磁场信息，只需提供先验重力加速度矢量，克服了上述地磁场在应用中的不足。
附图说明
图1是重力加速度矢量在全局坐标系G与载体坐标系B中的示意图；
图2是地磁场矢量在全局坐标系G与载体坐标系B中的示意图；
图3是全局坐标系G的构建示意图；
图4是本发明实施例提供的一种基于地球重力场与地磁场序贯修正的姿态解算方法流程图；
图5是在ROS验证平台下，利用本发明输出的载体姿态四元数波形图；
图6是在ROS验证平台下，利用本发明输出的载体姿态欧拉角波形图；
图7是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的俯仰角输出波形对比图；
图8是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的横滚角输出波形对比图；
图9是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的航向角输出波形对比图。
具体实施方式
下面结合附图对本发明做清楚完整的描述，以使本领域的技术人员在不需要作出创造性劳动的条件下，能够充分实施本发明。
如图1-4所示，本发明实施例提供的一种基于地球重力场与地磁场序贯修正的姿态解算方法，使用AH-100B AHRS传感器输出的三轴陀螺、三轴加速度、三轴磁场计数据，分别为ω、a和m，包括以下几个步骤：
步骤1：载体初始姿态四元数初始化，初始姿态四元数均方误差初始化；
步骤2：读取陀螺仪数据，并进行载体姿态四元数更新；
步骤3：读取加速度计数据，并对步骤2中更新的姿态四元数进行第一次修正；
步骤4：读取磁力计数据，并对步骤3中一次修正的姿态四元数进行第二次修正。
作为本发明的一个实施例，步骤1中姿态四元数与均方误差矩阵初始化为
Figure BDA0001641914320000081
P0＝I。
作为本发明的一个实施例，步骤2中读取陀螺仪数据，并进行载体姿态四元数更新，具体通过以下步骤实现：
(2a)在载体坐标系B，下从陀螺仪中获得载体角速度信息
Figure BDA0001641914320000091
(2b)根据所述载体角速度信息计算系统状态转移矩阵
Figure BDA0001641914320000092
(2c)陀螺仪噪声协方差矩阵设置
Figure BDA0001641914320000093
(2d)计算系统噪声协方差矩阵
Figure BDA0001641914320000094
(2e)载体姿态四元数状态一步预测
Figure BDA0001641914320000095
(2f)姿态四元数状态一步预测均方误差
Figure BDA0001641914320000096
作为本发明的一个实施例，步骤3中读取加速度计数据，并对步骤2中更新的姿态四元数进行第一次修正，具体通过以下步骤实现：
(3a)在载体坐标系B下从加速度计中获得载体角速度信息a＝[ax ay az]T；
(3b)根据所述载体重力加速度信息及先验重力加速度计算量测矩阵
Figure BDA0001641914320000097
(3c)加速度计噪声协方差矩阵设置
Figure BDA0001641914320000098
(3d)计算重力加速度量测噪声协方差矩阵
Figure BDA0001641914320000099
(3e)计算重力加速度修正的滤波增Kgk＝Pk/k-1(Hgk)T(HgkPk/k-1(Hgk)T+Rg′k)-1；
(3f)估计基于重力加速度修正的姿态四元数
Figure BDA00016419143200000910
(3g)基于重力加速度的姿态四元数状态估计均方误差Pgk＝(I-KgkHgk)Pk/k-1。
作为本发明的一个实施例，步骤4中读取磁力计数据，并对步骤3中一次修正的姿态四元数进行第二次修正，具体通过以下步骤实现：
(4a)在载体坐标系B下从磁力计中获得载体角速度信息m＝[mx my mz]T；
(4b)计算先验地磁场矢量h；
(4c)根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵
Figure BDA0001641914320000101
(4d)磁力计噪声协方差矩阵设置
Figure BDA0001641914320000102
(4e)计算地磁场矢量量测噪声协方差矩阵
Figure BDA0001641914320000103
(4f)计算地磁场矢量修正的滤波增Kmk＝Pgk(Hmk)T(HmkPgk(Hmk)T+Rm′k)-1；
(4g)估计基于地磁场矢量的姿态四元数状态
Figure BDA0001641914320000104
(4h)基于地磁场矢量的姿态四元数状态估计均方误差Pmk＝(I-KmkHmk)Pgk。
利用该姿态解算算法解算出的载体姿态四元数输出波形如图5所示，其w、x、y分量的精度为0.001，z分量的精度为0.002。载体姿态欧拉角输出波形如图6所示，其横滚与俯仰分量的精度为0.1°，航向分量的精度为0.2°。
将本发明与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角输出波形进行对比，对比图如图7-9所示。
以上对本发明的较佳实施例进行了描述，需要指出的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (1)

1.一种基于地球重力场与地磁场序贯修正的姿态解算方法，其特征在于，具体步骤为：
(1)根据航姿系统中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵，并利用离散化的四元数微分方程更新载体姿态四元数，具体过程是：
a.建立陀螺仪动力学模型：
Figure FDA0003462281280000011
即
Figure FDA0003462281280000012
其中，X＝[X0 X1 X2 X3]T表示全局坐标系G到载体坐标系B的旋转四元数，
Figure FDA0003462281280000013
表示在载体坐标系中测量的MEMS陀螺角速度信息，且
Figure FDA0003462281280000014
利用四元数微分方程的比卡求解法，使上式离散化可得
Figure FDA0003462281280000015
其中，Xk-1为tk-1时刻全局坐标系G到载体坐标系B的旋转四元数，Xk为tk时刻全局坐标系G到载体坐标系B的旋转四元数，
Figure FDA0003462281280000016
且
Figure FDA0003462281280000017
b.获得载体坐标系B下的载体角速度信息
Figure FDA0003462281280000018
c.根据所述载体角速度信息计算系统状态转移矩阵
Figure FDA0003462281280000021
c.载体姿态四元数状态一步预测
Figure FDA0003462281280000022
Figure FDA0003462281280000023
d.计算系统噪声协方差矩阵Qk
对于MEMS陀螺仪在微小时间间隔Δt，其真实的角增量Δθ°是无法测量的，假设在tk-1时刻到tk时刻的时间间隔内，陀螺仪的实际输出与真实角增量的误差为δθk/k-1，有
Figure FDA0003462281280000024
其中，Δθk/k-1表示陀螺仪的实际角增量，
Figure FDA0003462281280000025
表示陀螺仪的真实角增量；
相应的，实际状态转移矩阵与真实状态转移矩阵的误差为ΔΦk/k-1，则有
Figure FDA0003462281280000026
将上式代入步骤a中陀螺仪离散化的动力学模型中，可得
Figure FDA0003462281280000027
Figure FDA0003462281280000028
变形为
-δΘk/k-1Xk-1＝-Ξ(Xk-1)δθk/k-1
式中
Figure FDA0003462281280000029
设陀螺仪的输出噪声为ngyro，则有
Figure FDA00034622812800000210
则系统噪声协方差矩阵为：
Figure FDA0003462281280000031
其中，Σgyro为陀螺仪噪声协方差矩阵；
f.计算姿态四元数状态一步预测均方误差Pk/k-1
Figure FDA0003462281280000032
其中，Pk-1表示步骤(3)中的状态估计均方误差；
(2)根据航姿系统中MEMS加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正，具体过程是：
a.建立加速度计观测模型：
将三维矢量g与a看作零标量的四元数，则g与a间的变换关系可用四元数乘法表示为
Figure FDA0003462281280000033
其中，g为在全局坐标系G中测量的重力加速度，a为在载体坐标系B中测量的重力加速度，且满足||g||＝||a||＝1；
Figure FDA0003462281280000034
表示姿态四元数Xk的逆；
上式经过变换可得
M′(a)Xk-M(g)Xk＝0
其中，M′(a)与M(g)表示由零标量的四元数g与a构成的矩阵，如下
Figure FDA0003462281280000035
Figure FDA0003462281280000036
b.获得载体坐标系B下的载体重力加速度信息a＝[ax ay az]T；
c.根据所述载体重力加速度信息及先验重力加速度计算量测矩阵
Figure FDA0003462281280000041
d.计算重力加速度量测噪声协方差矩阵Rg′k
设加速度计的输出噪声为nacc，则有
nacc＝a-a°
其中，a°表示载体坐标系下加速度计的真实值；
实际量测矩阵与理想量测矩阵误差为
Figure FDA0003462281280000042
其中，
Figure FDA0003462281280000043
表示理想量测矩阵；
将上式代入步骤a中的加速度计观测模型，可得
0＝HgkXk-ΔHgkXk
-ΔHgkXk变形为
Figure FDA0003462281280000044
其中，
Figure FDA0003462281280000045
令
Figure FDA0003462281280000046
则量测噪声协方差矩阵为
Figure FDA0003462281280000047
其中，Σacc为加速度计噪声协方差矩阵；
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
Rg′k＝Rgk+βgI βg>0；
e.计算重力加速度修正的滤波增益Kgk
Kgk＝Pk/k-1(Hgk)T(HgkPk/k-1(Hgk)T+Rg′k)-1；
f.基于重力加速度的姿态四元数状态
Figure FDA0003462281280000051
估计
Figure FDA0003462281280000052
g.基于重力加速度的姿态四元数状态估计均方误差Pgk
Pgk＝(I-KgkHgk)Pk/k-1；
(3)根据航姿系统中MEMS磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正，具体过程是：
a.建立磁力计观测模型：
把三维矢量h与m看作零标量的四元数，则h与m间的变换关系可用四元数乘法表示为
Figure FDA0003462281280000053
其中，h为在全局坐标系G中测量的地磁场矢量，m为在载体坐标系B中测量的地磁场矢量，且满足||g||＝||a||＝1，
上式经过变换可得
M′(m)Xk-M(h)Xk＝0
其中，M′(m)与M(h)表示由零标量的四元数h与m构成的矩阵，如下
Figure FDA0003462281280000054
Figure FDA0003462281280000055
b.获得载体坐标系B下的载体地磁场矢量信息m＝[mx my mz]T；
c.计算先验地磁场矢量h
全局参考坐标系G的Gz轴选取重力加速度g的反方向，Gx轴选取地磁北极，Gy按照右手法则选取，则先验地磁场矢量h与全局参考坐标系G的GxoGz面重合即h＝[hx 0 hy]T，则有
Figure FDA0003462281280000061
其中，l＝[lx ly lz]T表示带有误差的先验地磁场矢量，
则有
Figure FDA0003462281280000062
式中
Γ＝lx 2+ly 2
d.根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵
Figure FDA0003462281280000063
e.计算地磁场矢量量测噪声协方差矩阵Rm′k
设磁力计的输出噪声为nmag，则有
nmag＝m-m°
其中，m°表示载体坐标系下加速度计的真实值；
实际量测矩阵与理想量测矩阵误差为
Figure FDA0003462281280000064
其中，
Figure FDA0003462281280000066
表示理想量测矩阵；
把上式代入步骤a中的磁力计观测模型，可得
0＝HmkXk-ΔHmkXk
-ΔHmkXk变形为
Figure FDA0003462281280000065
其中，
Figure FDA0003462281280000071
令
Figure FDA0003462281280000072
则量测噪声协方差矩阵为
Figure FDA0003462281280000073
其中，Σmag为磁力计噪声协方差矩阵；
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
Rm′k＝Rmk+βmI βm>0；
f.计算地磁场矢量修正的滤波增益Kmk
Kmk＝Pgk(Hmk)T(HmkPgk(Hmk)T+Rm′k)-1；
g.基于地磁场矢量的姿态四元数状态
Figure FDA0003462281280000074
估计
Figure FDA0003462281280000075
h.基于地磁场矢量的姿态四元数状态估计均方误差Pmk
Pmk＝(I-KmkHmk)Pgk
其中，I表示单位矩阵；
Figure FDA0003462281280000076
Pmk→Pk-1。
CN201810384477.2A 2018-04-26 2018-04-26 一种基于地球重力场与地磁场序贯修正的姿态解算方法 Expired - Fee Related CN108871319B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810384477.2A CN108871319B (zh) 2018-04-26 2018-04-26 一种基于地球重力场与地磁场序贯修正的姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810384477.2A CN108871319B (zh) 2018-04-26 2018-04-26 一种基于地球重力场与地磁场序贯修正的姿态解算方法

Publications (2)

Publication Number Publication Date
CN108871319A CN108871319A (zh) 2018-11-23
CN108871319B true CN108871319B (zh) 2022-05-17

Family

ID=64326785

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810384477.2A Expired - Fee Related CN108871319B (zh) 2018-04-26 2018-04-26 一种基于地球重力场与地磁场序贯修正的姿态解算方法

Country Status (1)

Country Link
CN (1) CN108871319B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110017837B (zh) * 2019-04-26 2022-11-25 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110174121A (zh) * 2019-04-30 2019-08-27 西北工业大学 一种基于地磁场自适应修正的航姿系统姿态解算方法
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
WO2012068359A2 (en) * 2010-11-17 2012-05-24 Hillcrest Laboratories, Inc. Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic
US8645063B2 (en) * 2010-12-22 2014-02-04 Custom Sensors & Technologies, Inc. Method and system for initial quaternion and attitude estimation
FR2976353B1 (fr) * 2011-06-07 2013-07-05 Movea Procede d'estimation simplifie de l'orientation d'un objet et centrale d'attitude mettant en oeuvre un tel procede
CN103363992B (zh) * 2013-06-29 2015-12-09 天津大学 基于梯度下降的四旋翼无人机姿态航向参考系统解算方法
CN103822633B (zh) * 2014-02-11 2016-12-07 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN106500695B (zh) * 2017-01-05 2019-02-01 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN107621261B (zh) * 2017-09-08 2020-09-08 常州大学 用于惯性-地磁组合姿态解算的自适应optimal-REQUEST算法
CN110887481B (zh) * 2019-12-11 2020-07-24 中国空气动力研究与发展中心低速空气动力研究所 基于mems惯性传感器的载体动态姿态估计方法

Also Published As

Publication number Publication date
CN108871319A (zh) 2018-11-23

Similar Documents

Publication Publication Date Title
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN101726295B (zh) 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
CN104374388B (zh) 一种基于偏振光传感器的航姿测定方法
CN109682377B (zh) 一种基于动态步长梯度下降的姿态估计方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
WO2018214227A1 (zh) 一种无人车实时姿态测量方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN107063254B (zh) 一种陀螺地磁组合的姿态解算方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111895988A (zh) 无人机导航信息更新方法及装置
CN105806363A (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN106370178A (zh) 移动终端设备的姿态测量方法及装置
US20170074689A1 (en) Sensor Fusion Method for Determining Orientation of an Object
CN107860382A (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20220517