CN109000639A - 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 - Google Patents

乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 Download PDF

Info

Publication number
CN109000639A
CN109000639A CN201810574982.3A CN201810574982A CN109000639A CN 109000639 A CN109000639 A CN 109000639A CN 201810574982 A CN201810574982 A CN 201810574982A CN 109000639 A CN109000639 A CN 109000639A
Authority
CN
China
Prior art keywords
moment
earth magnetism
estimation
parameter
tensor field
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.)
Granted
Application number
CN201810574982.3A
Other languages
English (en)
Other versions
CN109000639B (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201810574982.3A priority Critical patent/CN109000639B/zh
Publication of CN109000639A publication Critical patent/CN109000639A/zh
Application granted granted Critical
Publication of CN109000639B publication Critical patent/CN109000639B/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/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
    • 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/165—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 combined with non-inertial navigation instruments

Landscapes

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

Abstract

本发明属于水下地磁辅助导航领域，具体涉及到乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置。以误差四元数、陀螺比例因子误差以及陀螺常值偏置误差建立姿态估计装置的状态方程，捷联于运载体的矢量磁力计组，测量载体系下地磁张量场的5个独立分量；将其与载体系下地磁张量场独立分量的测量值作差，得到地磁张量场独立分量差值。利用卡尔曼滤波器进行估计与补偿，得到姿态四元数、陀螺比例因子及常值偏置的估计值；由姿态四元数确定运载体姿态。采用乘性误差四元数作为姿态估计变量，克服了状态协方差阵的奇异性问题；以地磁张量场独立分量差值为观测量，消减地磁背景场矢量方向对姿态估计器系统观测的影响。

Description

乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
技术领域
本发明属于水下地磁辅助导航领域，具体涉及到乘性误差四元数地磁张量场辅助陀螺的 姿态估计方法，本发明还涉及乘性误差四元数地磁张量场辅助陀螺的姿态估计装置。
背景技术
姿态估计器是用于确定运载体固联坐标系相对于参考坐标系的方位；不论是地面车辆、 海洋运载体还是飞行器，它们的自主导航与控制都需要一些姿态角信息。姿态估计有确定性 方法与递归方法。确定性方法有TRIAD算法、QUEST算法和FOAM算法等，它们需至少 测量两个矢量来确定载体姿态，对于多于两个矢量的测量，QUEST算法和FOAM算法可通过最优化代价函数提升姿态估计精度。递归方法应用了运动学/动力学模型，可通过单个矢量 的测量估计载体姿态。太阳敏感器、磁强计、星敏感器以及地平仪等是提供姿态参考矢量的 常用姿态敏感器，测量带宽较窄，输出存在低频噪声。测量惯性空间中载体角速率的三轴陀 螺受噪声和偏置的影响，其长期精度无法保证，因此常与姿态敏感器一起使用提高长期姿态 估计精度。
用于描述姿态的参量有方向余弦矩阵、欧拉角、旋转矢量及四元数等。四元数是非奇异 的、全局的姿态表示最小集，其描述的运动学方程是线性的，因此它被广泛用于描述姿态。 但是，四元数非线性滤波算法要实现连续意义上的姿态确定还存在几个问题：一是四元数的 双重性导致四元数估计的模糊；二是四元数在求取一步预测加权均值时不再满足其规范化要 求；三是四元数协方差计算的奇异性问题。如果不考虑四元数的规范化约束直接作滤波处理， 则滤波估计精度差，甚至会发散；目前已有一些应对方法。姿态误差描述参量的构造免除了 四元数的规范化约束，也降低系统维数[Markley F L.Attiude errorrepresentations for Kalman filtering.Journal of Guidance,Control andDynamics,2003,26(2):311-317]。罗德里格斯参数与 四元数之间的切换也是一种解决四元数规范化约束的方法[Crassidis J L,Markley F L. Unscented filtering forspacecraft attitude estimation.Journal of Guidance,Control and Dynamics,2003,26(4):536-542]，但多次不同姿态参数间的切换所引入的数值舍入误差在一定程度上降 低了滤波精度和稳定性。将四元数的规范化约束看作系统状态参数约束引入卡尔曼滤波框架， 但四元数约束本质上属于非线性约束，可将其线性化作为线性等式约束与滤波相结合，正如 Teixeira B O S在文章“Torres L,et al.State estimation for linearand nonlinear equality-constrained systems”中所述。这种线性化引入了截断误差，影响滤波精度。伪观测法 是构造一个方差为零的虚构观测值，使由卡尔曼滤波的更新方程求得的状态估计满足约束条 件，但使用伪观测方程有不足之处，J.D.Geeter在文献“Asmoothly constrained kalman filter, IEEE Trans.Pattern Anal.Machine Intell”中提到：首先，一个奇异的协方差阵可能会增加数值 问题；其次，状态约束式作为伪观测会增加融合算法的计算维数；最后，当约束条件为非线 性时，方法变得复杂。在T.Chia所著“Recursive parameter identification of constrained systems: An application toelectrically stimulated muscle”中提到，投影法是构造一个投影变换因子，不 断变换系统状态估计使其完全投影到约束面)。两步投影法较好地解决了伪观测法和投影法的 问题。两步投影法的第1步投影将无约束的状态估计分布投影到约束表面，使姿态估计分布满足四元数约束，但引起了姿态估计方差的下降；第2步投影将约束的状态估计分布投影到约束表面，使姿态估计均值满足四元数约束，同时补偿姿态估计方差。但求解含非线性约束的优化问题时计算量较大。
基于乘性误差四元数的卡尔曼滤波姿态估计仅估计乘性误差四元数的矢量部分，并不断 更新其参考四元数，将所估计的误差四元数限定在较小范围内，克服了四元数协方差的奇异 性问题。地磁场张量测量具有地磁矢量测量信息多的优点，又不象地磁标量测量那样对地磁 背景场矢量方向非常敏感。磁暴来源于太阳黑子活动，其值可达几百nT，但其源距离地球表 面很远；而地磁异常通常是由地下岩、矿埋藏目标物或地质构造引起的，异常源较浅。因此， 可认为地磁张量场只是由地磁异常产生；对于短基线(有的小于1m)的地磁张量场测量装 置，地磁背景场及磁暴对地磁张量测量的影响可忽略不计。
本发明提出一种基于乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置。姿态 估计装置采用乘性误差四元数作为姿态估计变量，克服了以四元数为姿态估计变量的状态协 方差阵的奇异性问题；以地磁张量场独立分量差值为观测量，消减地磁背景场矢量方向对姿 态估计器系统观测的影响；利用地磁张量场的姿态依赖关系结合陀螺运动学模型实现姿态信 息的连续递推估计，为水下运载体定姿提供一种自主的、隐蔽的途径；将陀螺常值偏置及比 例因子也作为待估计量，用它们的估计值校正陀螺测量值，提高了估计器的姿态估计精度。
发明内容
本发明的目的在于提供乘性误差四元数地磁张量场辅助陀螺的姿态估计的方法。
本发明的目的在于提供乘性误差四元数地磁张量场辅助陀螺的姿态估计的装置。
本发明的目的是这样实现的：本发明的一种基于乘性误差四元数地磁张量场辅助陀螺的 姿态估计方法及装置。误差四元数作为姿态的表征参数描述陀螺运动学方程，以地磁张量场 独立分量差值作为观测量，推导地磁张量场独立分量差值与误差四元数之间的依赖关系。以 误差四元数、陀螺比例因子误差以及陀螺常值偏置误差为状态变量建立地磁张量场独立分量 辅助陀螺姿态估计器的状态方程，以地磁张量场独立分量差值为观测量建立估计器的观测方 程。捷联于运载体的矢量磁力计组构成地磁张量场独立分量的测量装置，测量载体系下地磁 张量场的5个独立分量；依据组合导航系统输出的参考位置从地磁张量场数据库中提取当地 地理系下地磁场张量场的5个独立分量，将其与载体系下地磁张量场独立分量的测量值作差， 得到地磁张量场独立分量差值。利用卡尔曼滤波器对误差四元数、陀螺比例因子误差以及陀 螺常值偏置误差进行估计与补偿，得到姿态四元数、陀螺比例因子及常值偏置的估计值；由 姿态四元数确定运载体姿态。用陀螺常值偏置及比例因子的估计值校正陀螺测量值，提高了 姿态估计器的精度。图1给出了该姿态估计器的组成及整体流程图，其具体步骤如下：
步骤1：卡尔曼滤波器的初始化。
k＝0，给定四元数的初始值陀螺比例因子的初始值陀螺常值偏置的初始值 和系统初始状态方差矩阵Pk|k。
步骤2：根据k时刻陀螺比例因子的估计以及陀螺常值偏置的估计和三轴捷联陀螺 的测量输出根据式(1)计算k时刻运载体角速度的x、y和z轴分量的 估计
式中，和分别为k时刻陀螺x、y和z轴的比例因子的估计，和分别为k时刻陀螺x、y和z轴的常值偏置的估计。
步骤3：由步骤2中的和根据式(2)计算四元数的一步预测值
式中，
Δtk为陀螺采样周期。
步骤4：由及根据式 (3)计算Fk。
式中，
为的第j个元素。
步骤5：由步骤4中的Fk，根据式(4)计算Φk+1|k。
式中，Ts为滤波器的滤波周期，这里设为陀螺采样周期；nF∈Z+为求和项数。
步骤6：计算一步预测误差方差阵Pk+1|k。
式中，qk为系统过程噪声 方差阵。
步骤7：根据组合导航系统的指示位置，从预先存储的地磁张量场独立分量数据库中提取k+1 时刻地理系下地磁张量场独立分量及
步骤8：分别按式(6)、式(7)、式(8)、式(9)和式(10)计算及
式中，为第j个元素。
步骤9：按式(11)计算Hk+1。
步骤10：按式(12)计算滤波增益阵Kk+1。
式中，Rk+1为姿态估计器系统的观测噪声方差阵，符号T表示转置。
步骤11：按式(13)计算估计误差方差矩阵Pk+1|k+1。
Pk+1|k+1＝[I9×9-Kk+1Hk+1]Pk+1|k (13)
步骤12：根据式(14)计算得到k+1时刻载体系下地磁张量场独立分量 和
式中，Lx和Ly分别为xb和yb方向上测量基线长，和分别为A点处的地磁场x、 y和z分量的测量值，和分别为B点处的地磁场x、y和z分量的测量值，和分别为C点处的地磁场y和z分量的测量值，和分别为D点处的地磁场y和z分量的测量值，如图2所示。
步骤13：根据式(15)计算地磁张量场独立分量测量差值和
步骤14：根据式(16)计算系统状态估计
步骤15：按式(17)计算k+1时刻的姿态四元数的估计。
式中，为k+1时刻的误差四元数， 表示四元数乘法运算。
步骤16：按式(18)计算k+1时刻的陀螺比例因子的估计
步骤17：按式(19)计算k+1时刻的陀螺常值偏置的估计
步骤18：将代入式(20)计算k+1时刻的姿态矩阵的估计
步骤19：由的矩阵元素和计算k+1时刻的姿态角估计。 步1)：将代入式(21)计算k+1时刻的俯仰角的估计
步2)：将和代入式(22)计算k+1时刻的横滚角的主值
步3)：计算k+1时刻的俯仰角的估计
当时，当且时，当且 时
步4)：将和代入式(23)计算k+1时刻的航向角的主值
步5)：计算k+1时刻的航向角的估计
当且时，当且时，当时，当且时，当且时，
步骤20：k＝k+1，转步骤2进行下一时刻的姿态角、陀螺常值偏置及比例因子的估计。
本发明的有益效果在于：提出的基于误差四元数的地磁张量场独立分量辅助陀螺的姿态 估计器以乘性误差四元数为姿态估计变量，克服了四元数姿态变量的状态协方差阵奇异的问 题；以磁场张量场独立分量差值为观测量，消减地磁背景场方向对估计器系统观测的影响； 将陀螺常值偏置及比例因子也作为估计变量，用它们的估计值校正陀螺测量值，提高了估计 器的姿态估计精度；能在较大的磁测噪声及初始姿态误差的情况下对姿态角、陀螺常值偏置 及比例因子进行较精确地估计，对磁测噪声水平及初始姿态误差的要求低，完全可以使用磁 通门等类型的磁力计作为地磁张量场测量单元、惯性导航系统输出姿态信息作为算法初始解， 装置价格低廉、实用性强。利用96个实球体磁性体和64个长方体形磁性体的张量场模拟地 磁张量场，用地磁张量场独立分量作为量测量及误差四元数作为描述姿态误差的参数对姿态 角、陀螺常值偏置及比例因子进行滤波估计；考察了不同磁传感器噪声和初始姿态误差角对 姿态角、陀螺常值偏置及比例因子估计的影响。多组仿真实验结果表明姿态角、陀螺常值偏 置及比例因子估计的绝对误差的平均值都随磁传感器测量噪声的增大而线性增加；姿态角、 陀螺常值偏置及比例因子估计的绝对误差的平均值不随初始姿态误差的增大而显著变化。当 矢量磁力计测量噪声的标准差在10nT以内及姿态初值偏离其真值30°以内，该估计器都能很 好地对姿态角、陀螺常值偏置及比例因子进行估计；该估计器对磁测噪声及初始姿态误差的 要求低，完全可以使用磁通门磁力计作为地磁张量场测量单元，惯性导航系统输出姿态信息 作为算法初始解，装置价格低廉、实用性强；本发明还可对三轴捷联陀螺的常值偏置及比例 因子等参数进行在线标定。
附图说明
图1是姿态估计装置的组成及整体流程图。
图2是地磁张量场测量的矢量磁力计配置图。
图3(a)是地磁张量场的独立分量第一参数参考图及载体航迹。
图3(b)是地磁张量场的独立分量第二参数参考图及载体航迹。
图3(c)是地磁张量场的独立分量第三参数参考图及载体航迹。
图3(d)是地磁张量场的独立分量第四参数参考图及载体航迹。
图3(e)是地磁张量场的独立分量第五参数参考图及载体航迹。
图4是未滤波估计的陀螺直接解算姿态角的绝对误差。
图5(a)是在偏航角初始误差为30°，俯仰角初始误差为40°和横滚角初始误差为-20°， 磁传感器噪声标准差为0.1nT的情况下姿态角估计的绝对误差曲线。
图5(b)是在偏航角初始误差为30°，俯仰角初始误差为40°和横滚角初始误差为-20°， 磁传感器噪声标准差为0.1nT的情况下常值偏置估计的绝对误差曲线。
图5(c)是在偏航角初始误差为30°，俯仰角初始误差为40°和横滚角初始误差为-20°， 磁传感器噪声标准差为0.1nT的情况下比例因子估计的绝对误差曲线。
图6(a)姿态角估计的绝对误差的平均值随磁力计测量噪声的标准差的变化关系图。
图6(b)是常值偏置估计的绝对误差的平均值随磁力计测量噪声的标准差的变化关系图。
图6(c)是比例因子估计的绝对误差的平均值随磁力计测量噪声的标准差的变化关系图。
图7(a)是姿态角估计的绝对误差的平均值随初始姿态误差的变化关系图。
图7(b)是常值偏置估计的绝对误差的平均值随初始姿态误差的变化关系图。
图7(c)是比例因子估计的绝对误差的平均值随初始姿态误差的变化关系图。
具体实施方式：
下面结合附图对本发明做进一步描述：
步骤1：卡尔曼滤波器的初始化。
k＝0，根据初始姿态角误差给定初始姿态角，由此计算四元数的初始值给定陀螺比例 因子的初始值陀螺常值偏置的初始值和系统初始状态方差矩阵Pk|k。
步骤2：根据k时刻的陀螺比例因子的估计以及陀螺常值偏置的估计和三轴捷 联陀螺的测量输出和计算
三轴捷联陀螺的测量模型为
其中，和分别为三轴捷联陀螺的x、y和z轴的测量输出，Kgx、Kgy和Kgz为分别为陀螺x、y和z轴的比例因子，和分别为运载体角速度的x、y和z轴分 量，和分别为陀螺x、y和z轴的常值偏置，和分别为三轴捷联陀 螺的x、y和z轴的测量噪声。
由式(1)得到k时刻运载体角速度估计的计算式为
其中，和分别为运载体角速度的x、y和z轴分量的估计，和分别为陀螺x、y和z轴的比例因子的估计，和分别为陀螺x、y和z轴的 常值偏置的估计。
步骤3：由步骤2中的根据式(3)计算四元数的一步预测值
其中，Δtk为陀螺采样周期。
步骤4：由及根 据式(4)计算矩阵Fk。
式中，
为的第j个元素。
步骤5：由步骤4中的Fk，根据式(5)计算Φk+1|k。
式中，Ts为滤波器的滤波周期，这里设为陀螺采样周期；nF∈Z+为求和项数。
步骤6：根据式(6)计算一步预测误差方差阵Pk+1|k。
式中，
qk为系统过程噪声方差阵。
步骤7：根据组合导航系统的指示位置，从预先存储的地磁张量场独立分量数据库中提 取k+1时刻地理系下地磁张量场独立分量及
步骤8：在地理系和载体下的地磁张量场之间的变换关系为
其中，Tn和Tb分别为地理系和载体下的地磁张量场，符号T表示转置，为姿态矩阵，可 表示为
其中q1、q2和q3为四元数的矢量部分，q4为四元数的标量部分。
根据式(7)和(8)可以推得，k+1时刻载体系下地磁张量场独立分量的估计 及由和步骤7得到的k+1时刻地理系下地磁张量场独立分量，分别 按式(9)、式(10)、式(11)、式(12)和式(13)计算。
式中，为第j个元素。
步骤9：由步骤8得到的k+1时刻载体系下地磁张量场独立分量的估计，按式(14)计算Hk+1。
步骤10：由Hk+1和一步预测误差方差阵Pk+1|k，根据式(15)计算滤波增益阵Kk+1。
式中，Rk+1为k+1时刻的姿态估计器系统的观测噪声方差阵，符号T表示转置。
步骤11：按式(16)计算估计误差方差矩阵Pk+1|k+1。
Pk+1|k+1＝[I9×9-Kk+1Hk+1]Pk+1|k (16)
其中，I9×9为9×9的单位阵。
步骤12：由矢量磁力计对地磁场分量的测量值，计算k+1时刻载体系下地磁张量场独立 分量的测量值。
两对矢量磁力计捷联于运载体，对应的载体系为obxbybzb，用两对矢量磁力计的十字叉配 置测量地磁张量场独立分量，如图2所示。一对三轴磁力计分别放置在xb轴的A、B两点， 分别测量A和B两点处的地磁场分量和另一对二轴磁力计分别置于yb轴的C、D两点，分别测量C和D两点处的地磁场分量和箭头表示每个磁力计的敏感轴方向。地磁张量场独立分量 和的计算式为
式中，Lx和Ly分别为xb和yb方向上测量基线长。
由和根据式(17)计算得 到k+1时刻载体系下地磁张量场独立分量的测量值和
步骤13：根据k+1时刻载体系下地磁张量场独立分量的测量值及估计值，由式(18)计 算k+1时刻的地磁张量场独立分量的测量差值和
步骤14：由k+1时刻的地磁张量场独立分量测量差值，根据式(19)计算该时刻的系统 状态估计。
其中，为k+1时刻的系统状态估计。
步骤15：由k+1时刻的误差四元数估计值及四元数的一步预测值，按式(20)计算该时 刻的姿态四元数的估计。
式中，表示的是由和构成的k+1时刻的误差四元数估 计值， 表示四元数乘法运算。
步骤16：由上一时刻的陀螺比例因子估计值及k+1时刻的陀螺比例因子误差估计值，按 式(21)计算该时刻的陀螺比例因子估计值
步骤17：由上一时刻的陀螺常值偏置估计值及k+1时刻的陀螺常值偏置误差估计值，按 式(22)计算该时刻的陀螺常值偏置的估计
步骤18：将k+1时刻的姿态四元数的估计代入式(23)，计算k+1时刻的姿态矩阵 的估计
步骤19：由的矩阵元素和计算k+1时刻的姿态 角估计。
设运载体的航向角为ψ(习惯上以北偏东为正)，俯仰角为θ，横滚角为γ，取地理坐标系的三 个坐标轴xn、yn、zn的指向依次为北东地，如图3所示。则机体坐标系b与地理坐标系n的 关系描述为，第1步：Oxnynzn绕zn轴旋转ψ得Ox1y1z1，第2步：Ox1y1z1绕x1轴旋转θ得Ox2y2z2， 第3步：Ox2y2z2绕y2轴旋转γ得Oxbybzb，则姿态矩阵为
步1)：将步骤18得到的矩阵元素代入式(25)计算k+1时刻的俯仰角的估计
步2)：将和代入式(22)计算k+1时刻的横滚角的主值
步3)：计算k+1时刻的俯仰角的估计
当时，当且时，当且 时
步4)：将和代入式(23)计算k+1时刻的航向角的主值
步5)：计算k+1时刻的航向角的估计
当且时，当且时，当时，当且时，当且时，
步骤20：k＝k+1，转步骤2进行下一时刻的姿态角、陀螺常值偏置及比例因子的估计。
在Matlab数值仿真平台下，对该方法进行仿真实验：
用96个实球体磁性体和64个长方体形磁性体产生的张量场作为地磁张量场数据库，即 为地磁张量场参考图。运载体在x和y方向运动模型的状态参数X和Y分别为
式中，x和y为位置坐标。根据Robert A.Singer给出的运动模型，x和y方向运动状态参 数的微分方程为
式中，αx和αy分别为x和y方向 加速度的反相关时间常数，wx和wy分别为x和y方向加速度的驱动白噪声，它们的方差分别 为和和分别为x和y方向加速度的噪声方差。
地磁张量场参考图坐标原点的经纬度分别为120°和28°。选取αx＝αy＝0.05s-1，运载体相对于地磁张量场参考图坐标原点的初始位置x0＝400m和 y0＝400m，初始速度为和初始加速度为零。采样时间间隔为0.1s， 总航行时间为900s。地磁张量场独立分量参考图及载体航迹、独立分量参考图及载体航迹、独立分量参考图及载体航迹、独立分量参考图及载体航迹、独立分量参考图及载体航迹分别如图3(a)、图3(b)、图3(c)、图3(d)和图3(e)所示，其中蓝色曲线 为航迹线，参考图的网格数为1024×1024，地磁张量场独立分量的单位为nT/m。运载体的姿 态角变化规律为
式中，ψ为偏航角，θ为俯仰角，γ为横滚角。
地磁张量场测量装置的x和y方向的基线长分别为Lx＝1m，Ly＝1m，每个矢量磁力计的 测量噪声为零均值和相同标准差的高斯白噪声。陀螺的测量噪声为零均值的高斯白噪声，均 方差为0.001°·s-1，真实的三轴比例因子分别为1.23，0.88和1.29，真实的三轴常值偏置分别 为0.02°·s-1、0.03°·s-1和-0.01°·s-1。定义偏航角估计的绝对误差为 为偏航角 的估计值；俯仰角估计的绝对误差为 为俯仰角的估计值；横滚角估计的绝对误 差为 为横滚角的估计值；陀螺x轴、y轴和z轴常值偏置估计的绝对误差分别 为和陀螺x轴、y轴 和z轴比例因子估计的绝对误差分别为和 当不进行姿态估计，仅用陀螺输出计算姿态角的绝对误差如图4所 示,其中为未滤波估计的偏航角绝对误差的曲线，为未滤波估计的俯仰 角绝对误差的曲线，为未滤波估计的横滚角绝对误差的曲线。由图4可知，未滤波 估计的偏航角绝对误差均值和标准差分别为26.63153°和11.03139°，未滤波估计的俯仰角绝对 误差均值和标准差分别为40.98711°和12.57206°，未滤波估计的横滚角绝对误差均值和标准差 分别为33.42213°和15.57585°。在偏航角初始误差为30°，俯仰角初始误差为40°和横滚角初 始误差为-20°，磁传感器噪声标准差为0.1nT的情况下得到姿态角估计的绝对误差曲线如图5 (a)所示，其中为的曲线，为的曲线，为的曲线； 常值偏置估计的绝对误差曲线如图5(b)所示，其中为的曲线，为的曲线，为的曲线；比例因子估计的绝对误差曲线如图5(c)所示， 其中为的曲线，为的曲线，为的曲线。由 图5可知，姿态角估计的绝对误差快速地收敛到很小的值，其中偏航角估计绝对误差的平均 值和标准差分别为0.41931°和0.51172°；俯仰角估计绝对误差的平均值和标准差分别为 0.42358°和0.53211°；横滚角估计绝对误差的平均值和标准差分别为0.38378°和0.48549°；常 值偏置估计的绝对误差快速地收敛到很小的值，其中x轴常值偏置估计绝对误差的平均值和 标准差分别为0.00015473°·s-1和0.0015762°·s-1，y轴常值偏置估计绝对误差的平均值和标准差分别为0.00078850°·s-1和0.0086928°·s-1，z轴常值偏置估计绝对误差的平均值和标准差 分别为0.00043613°·s-1和0.0074163°·s-1；比例因子估计的绝对误差快速地收敛到很小的值， 其中x轴比例因子估计绝对误差的平均值和标准差分别为0.0053499和0.017448，y轴比例因 子估计绝对误差的平均值和标准差分别为0.010074和0.021851，z轴比例因子估计绝对误差 的平均值和标准差分别为0.0039969和0.0211。
姿态角估计的绝对误差的平均值随磁力计测量噪声的标准差的变化关系如图6(a)所示， 其中为偏航角绝对误差的平均值与测量噪声的标准差标准差之间的关系曲线， 为俯仰角绝对误差的平均值与磁传感器标准差之间的关系曲线，为横滚 角绝对误差的平均值与测量噪声的标准差标准差之间的关系曲线。常值偏置估计的绝对误差 的平均值随磁力计测量噪声的标准差之间的关系如图6(b)所示，其中为x轴常 值偏置绝对误差的平均值与测量噪声的标准差之间的关系曲线，为y轴常值偏置绝 对误差的平均值与测量噪声的标准差之间的关系曲线，为z轴常值偏置绝对误差的 平均值与测量噪声的标准差之间的关系曲线。比例因子估计的绝对误差的平均值随磁力计测 量噪声的标准差的变化关系如图6(c)所示，其中为x轴比例因子绝对误差的平 均值与测量噪声的标准差之间的关系曲线，为y轴比例因子绝对误差的平均值与 测量噪声的标准差之间的关系曲线，为z轴比例因子绝对误差的平均值与测量噪声 的标准差之间的关系曲线。由图6可知，姿态角估计的绝对误差的平均值随磁传感器测量噪 声的标准差呈线性增长，拟合得到偏航角绝对误差的增长率为0.33425±0.00809(°/nT)，俯 仰角绝对误差的增长率为0.16733±0.00476(°/nT)，横滚角绝对误差的增长率为 0.12959±0.00322(°/nT)；常值偏置估计的绝对误差的平均值随测量噪声的标准差的增加而 增长，线性拟合得到x轴常值偏置绝对误差的增长率为(6.17843±0.53583)×10-5(°/s/nT)，y 轴常值偏置绝对误差的增长率为(1.07503±0.04862)×10-4(°/s/nT)，z轴常值偏置绝对误差的 增长率为(5.43278±0.49450)×10-5(°/s/nT)；比例因子估计的绝对误差的平均值随磁传感器 测量噪声的标准差呈线性增长，拟合得到x轴比例因子绝对误差的增长率为 (1.288±0.031)×10-2(nT-1)，y轴比例因子绝对误差的增长率为(0.443±0.016)×10-2(nT-1)，z 轴比例因子绝对误差的增长率为(1.097±0.015)×10-2(nT-1)。
姿态角估计的绝对误差的平均值随初始姿态误差的变化关系如图7(a)所示，其中为偏航角绝对误差的平均值与初始姿态误差之间的关系曲线，为俯仰角绝对误差 的平均值与初始姿态误差之间的关系曲线，为横滚角绝对误差的平均值与初始姿态 误差之间的关系曲线。常值偏置估计的绝对误差的平均值随初始姿态误差的变化关系如图7 (b)所示，其中为x轴常值偏置绝对误差的平均值与初始姿态误差之间的关系曲 线，为y轴常值偏置绝对误差的平均值与初始姿态误差之间的关系曲线，为z轴常值偏置绝对误差的平均值与初始姿态误差之间的关系曲线。比例因子估计的绝对误 差的平均值随初始姿态误差的变化关系如图7(c)所示，其中为x轴比例因子绝 对误差的平均值与初始姿态误差之间的关系曲线，为y轴比例因子绝对误差的平 均值与初始姿态误差之间的关系曲线，为z轴比例因子绝对误差的平均值与初始姿 态误差之间的关系曲线。由图7可知，姿态角、常值偏置及比例因子估计的绝对误差的平均 值都不随初始姿态误差的增大而发生显著变化。
本发明有益效果说明如下：
利用96个实球体磁性体和64个长方体形磁性体的张量场模拟地磁张量场，用地磁张量 场独立分量作为量测量及误差四元数作为描述姿态误差的参数对姿态角、陀螺常值偏置及比 例因子进行滤波估计；考察了不同磁传感器噪声和初始姿态误差角对姿态角、陀螺常值偏置 及比例因子估计的影响。多组仿真实验结果表明姿态角、陀螺常值偏置及比例因子估计的绝 对误差的平均值都随磁传感器测量噪声的增大而线性增加；姿态角、陀螺常值偏置及比例因 子估计的绝对误差的平均值不随初始姿态误差的增大而显著变化。
当矢量磁力计测量噪声的标准差在10nT以内及姿态初值偏离其真值30°以内，该估计器都能 很好地对姿态角、陀螺常值偏置及比例因子进行估计；该估计器对磁测噪声及初始姿态误差 的要求低，完全可以使用磁通门磁力计作为地磁张量场测量单元，惯性导航系统输出姿态信 息作为算法初始解，装置价格低廉、实用性强；本发明还可对三轴捷联陀螺的常值偏置及比 例因子等参数进行在线标定。

Claims (9)

1.乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，本方法包括以下步骤：
(1)卡尔曼滤波器的初始化，令k＝0，根据四元数的估计值k时刻的陀螺比例因子的估计值陀螺常值偏置的估计值和三轴捷联陀螺的测量输出：x轴角速度测量值y轴角速度测量值z轴角速度测量值计算k时刻运载体角速度的x、y和z轴分量的估计：x轴角速度估计值y轴角速度估计值z轴角速度估计值
(2)根据k时刻运载体角速度的x、y和z轴分量的估计，计算四元数的一步预测值
(3)由陀螺比例因子的估计及x轴角速度测量值y轴角速度测量值z轴角速度测量值x轴陀螺比例因子的估计y轴陀螺比例因子的估计z轴陀螺比例因子的估计x轴陀螺常值偏置的估计y轴陀螺常值偏置的估计z轴陀螺常值偏置的估计计算第一参数Fk；
(4)由步骤(3)中的第一参数Fk，计算第二参数Φk+1|k,；
(5)由步骤(4)中的第二参数Φk+1|k，计算一步预测误差方差阵Pk+1|k，根据组合导航系统的指示位置，从预先存储的地磁张量场独立分量数据库中提取k+1时刻地理系下地磁张量场独立分量第一参数k+1时刻地理系下地磁张量场独立分量第二参数k+1时刻地理系下地磁张量场独立分量第三参数k+1时刻地理系下地磁张量场独立分量第四参数及k+1时刻地理系下地磁张量场独立分量第五参数
(6)根据k+1时刻地理系下地磁张量场独立分量第一参数k+1时刻地理系下地磁张量场独立分量第二参数k+1时刻地理系下地磁张量场独立分量第三参数k+1时刻地理系下地磁张量场独立分量第四参数及k+1时刻地理系下地磁张量场独立分量第五参数计算k+1时刻地理系下地磁张量场独立分量的第一参数估计k+1时刻地理系下地磁张量场独立分量的第二参数估计k+1时刻地理系下地磁张量场独立分量的第三参数估计k+1时刻地理系下地磁张量场独立分量的第四参数估计及k+1时刻地理系下地磁张量场独立分量的第五参数估计
(7)根据k+1时刻地理系下地磁张量场独立分量的第一参数估计k+1时刻地理系下地磁张量场独立分量的第二参数估计k+1时刻地理系下地磁张量场独立分量的第三参数估计k+1时刻地理系下地磁张量场独立分量的第四参数估计及k+1时刻地理系下地磁张量场独立分量的第五参数估计计算k+1时刻的地磁场Hk+1；
(8)根据k+1时刻的地磁场Hk+1，计算滤波增益阵Kk+1；
(9)根据k+1时刻的地磁场Hk+1、滤波增益阵Kk+1、一步预测误差方差阵Pk+1|k，计算估计误差方差矩阵Pk+1|k+1，计算得到k+1时刻载体系下地磁张量场独立分量第一参数k+1时刻载体系下地磁张量场独立分量第二参数k+1时刻载体系下地磁张量场独立分量第三参数k+1时刻载体系下地磁张量场独立分量第四参数和k+1时刻载体系下地磁张量场独立分量第五参数
(10)根据k+1时刻地理系下地磁张量场独立分量的第一参数估计k+1时刻地理系下地磁张量场独立分量的第二参数估计k+1时刻地理系下地磁张量场独立分量的第三参数估计k+1时刻地理系下地磁张量场独立分量的第四参数估计及k+1时刻地理系下地磁张量场独立分量的第五参数估计和k+1时刻载体系下地磁张量场独立分量第一参数k+1时刻载体系下地磁张量场独立分量第二参数k+1时刻载体系下地磁张量场独立分量第三参数k+1时刻载体系下地磁张量场独立分量第四参数和k+1时刻载体系下地磁张量场独立分量第五参数计算地磁张量场独立分量第一参数测量差值地磁张量场独立分量第二参数测量差值地磁张量场独立分量第三参数测量差值地磁张量场独立分量第四参数测量差值和地磁张量场独立分量第五参数测量差值
(11)根据地磁张量场独立分量第一参数测量差值地磁张量场独立分量第二参数测量差值地磁张量场独立分量第三参数测量差值地磁张量场独立分量第四参数测量差值和地磁张量场独立分量第五参数测量差值，计算系统状态估计
(12)根据系统状态估计一步预测误差方差阵Pk+1|k、k时刻的陀螺比例因子的估计以及k时刻陀螺常值偏置的估计计算k+1时刻的姿态四元数的估计k+1时刻的陀螺比例因子的估计k+1时刻的陀螺常值偏置的估计
(13)根据k+1时刻的姿态四元数的估计计算k+1时刻的姿态矩阵的估计
(14)由k+1时刻的姿态矩阵的估计计算k+1时刻的姿态角估计；
(15)令k＝k+1，转步骤2进行下一时刻的姿态角、陀螺常值偏置及比例因子的估计。
2.根据权利要求1所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，所述步骤(1)的k时刻运载体角速度的x、y和z轴分量的估计：x轴角速度估计值y轴角速度估计值z轴角速度估计值按下式计算
式中，为k时刻陀螺x轴的比例因子的估计值，为k时刻陀螺y轴的比例因子的估计值，为k时刻陀螺z轴的比例因子的估计值，为k时刻陀螺x轴的常值偏置的估计值、为k时刻陀螺y轴的常值偏置的估计值，为k时刻陀螺z轴的常值偏置的估计值，x轴角速度测量值y轴角速度测量值z轴角速度测量值
步骤(2)所述的四元数的一步预测值按下式进行计算：
式中，Δtk为陀螺采样周期，为k时刻四元数的估计值；
步骤(3)所述的第一参数Fk按下式计算：
式中，
为四元数的估计值的第j个元素；
由第一参数Fk，按下式计算步骤(4)所述的第二参数Φk+1|k：
式中，Ts为滤波器的滤波周期，这里设为陀螺采样周期，Fk为第一参数，nF∈Z+为求和项数；
按下式计算步骤(5)所述的一步预测误差方差阵Pk+1|k：
式中，qk为系统过程噪声方差阵，Φk+1|k为第二参数，Ts为滤波器的滤波周期，这里设为陀螺采样周期，为四元数的估计值。
3.根据权利要求2所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，按下式计算步骤(6)所述的k+1时刻地理系下地磁张量场独立分量的第一参数估计
式中，为四元数的一步预测值第j个元素
按下式计算步骤(6)所述的k+1时刻地理系下地磁张量场独立分量的第二参数估计
式中，为四元数的一步预测值第j个元素，
按下式计算步骤(6)所述的k+1时刻地理系下地磁张量场独立分量的第三参数估计
式中，为四元数的一步预测值第j个元素，
按下式计算步骤(6)所述的k+1时刻地理系下地磁张量场独立分量的第四参数估计
式中，为四元数的一步预测值第j个元素，
按下式计算步骤(6)所述的k+1时刻地理系下地磁张量场独立分量的第五参数估计
式中，为四元数的一步预测值第j个元素，
按下式计算步骤(7)所述的k+1时刻的地磁场Hk+1：
式中为k+1时刻地理系下地磁张量场独立分量的第一参数估计、为k+1时刻地理系下地磁张量场独立分量的第二参数估计、为k+1时刻地理系下地磁张量场独立分量的第三参数估计、为k+1时刻地理系下地磁张量场独立分量的第四参数估计、为k+1时刻地理系下地磁张量场独立分量的第五参数估计
按下式计算步骤(8)所述的滤波增益阵Kk+1：
式中，Hk+1为k+1时刻的地磁场，Rk+1为姿态估计器系统的观测噪声方差阵，符号T表示转置，为四元数的一步预测值。
4.根据权利要求3所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，按下式计算步骤(9)所述的估计误差方差矩阵Pk+1|k+1：
Pk+1|k+1＝[I9×9-Kk+1Hk+1]Pk+1|k
式中，I9×9代表9×9的单位矩阵，Kk+1为滤波增益阵，Hk+1为k+1时刻的地磁场，Pk+1|k为一步预测误差方差阵；
根据下面方程式分别计算步骤(9)所述的k+1时刻载体系下地磁张量场独立分量第一参数k+1时刻载体系下地磁张量场独立分量第二参数k+1时刻载体系下地磁张量场独立分量第三参数k+1时刻载体系下地磁张量场独立分量第四参数和k+1时刻载体系下地磁张量场独立分量第五参数
式中，Lx为xb方向上测量基线长，Ly为yb方向上测量基线长，为A点处的地磁场x分量的测量值、为A点处的地磁场y分量的测量值和为A点处的地磁场z分量的测量值，为B点处的地磁场x分量的测量值、为B点处的地磁场y分量的测量值和为B点处的地磁场z分量的测量值，为C点处的地磁场y分量的测量值和分别为C点处的地磁场z分量的测量值，为D点处的地磁场y分量的测量值和为D点处的地磁场z分量的测量值；
根据下式分别计算步骤(10)所述的地磁张量场独立分量第一参数测量差值地磁张量场独立分量第二参数测量差值地磁张量场独立分量第三参数测量差值地磁张量场独立分量第四参数测量差值和地磁张量场独立分量第五参数测量差值
式中
根据步骤(11)所述的下式计算系统状态估计
式中，为地磁张量场独立分量第一参数测量差值、为地磁张量场独立分量第二参数测量差值、为地磁张量场独立分量第三参数测量差值、为地磁张量场独立分量第四参数测量差值、为地磁张量场独立分量第五参数测量差值。
5.根据权利要求4所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，按下式计算步骤(12)所述的k+1时刻的姿态四元数的估计：
式中，为k+1时刻的误差四元数， 表示四元数乘法运算，
按下式计算步骤(12)所述的k+1时刻的陀螺比例因子的估计
式中为陀螺比例因子的估计值，为系统状态估计中的第二个元素；
按下式计算步骤(12)所述的k+1时刻的陀螺常值偏置的估计
式中为陀螺常值偏置的估计值，为系统状态估计中的第三个元素。
6.根据权利要求5所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，按下式计算步骤(13)所述的k+1时刻的姿态矩阵的估计
式中表示第i行第j列的姿态矩阵中的元素；
由的矩阵元素和计算步骤(13)所述的k+1时刻的姿态角估计：
(14.1)根据下式计算k+1时刻的俯仰角的估计
(14.2)按下式计算k+1时刻的横滚角的主值
(14.3)计算k+1时刻的俯仰角的估计
当时，当且时，当且时
(14.4)按下式计算k+1时刻的航向角的主值
(14.5)按下式计算k+1时刻的航向角的估计
当且时，当且时，当时，当且时，当且时，
7.根据权利要求6所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于，令k＝k+1，转步骤(2)进行下一时刻的姿态角、陀螺常值偏置及比例因子的估计。
8.根据权利要求1所述的乘性误差四元数地磁张量场辅助陀螺的姿态估计方法，其特征在于：以乘性误差四元数为姿态估计变量，以磁场张量场独立分量差值为观测量，将陀螺常值偏置及比例因子也作为估计变量，用它们的估计值校正陀螺测量值。
9.乘性误差四元数地磁张量场辅助陀螺的姿态估计装置，包括地磁张量场测量装置以及姿态估计计算装置，其特征在于：计算装置中包含组合导航系统、地磁张量场独立分量数据库、卡尔曼滤波器的初始化模块、系统观测量的计算程序、基于乘性误差四元数和地磁张量场的姿态估计滤波算法模块、陀螺常值偏置与比例因子估计值输出模块、四元数估计值输出模块以及姿态角估计值输出模块。
CN201810574982.3A 2018-06-05 2018-06-05 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 Active CN109000639B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810574982.3A CN109000639B (zh) 2018-06-05 2018-06-05 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810574982.3A CN109000639B (zh) 2018-06-05 2018-06-05 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置

Publications (2)

Publication Number Publication Date
CN109000639A true CN109000639A (zh) 2018-12-14
CN109000639B CN109000639B (zh) 2022-01-07

Family

ID=64599952

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810574982.3A Active CN109000639B (zh) 2018-06-05 2018-06-05 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置

Country Status (1)

Country Link
CN (1) CN109000639B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109990776A (zh) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN110849391A (zh) * 2019-11-01 2020-02-28 上海卫星工程研究所 矢量磁力仪与星敏感器非正交安装矩阵标定系统及方法
CN111678515A (zh) * 2020-04-28 2020-09-18 北京三快在线科技有限公司 设备状态估计方法、装置、电子设备及存储介质
CN112033405A (zh) * 2020-08-10 2020-12-04 北京摩高科技有限公司 一种室内环境磁异常实时修正与导航方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
US20120136604A1 (en) * 2010-11-30 2012-05-31 Industrial Technology Research Institute Method and apparatus for 3d attitude estimation
CN103630139A (zh) * 2013-12-17 2014-03-12 哈尔滨工程大学 一种基于地磁梯度张量测量的水下载体全姿态确定方法
CN104567871A (zh) * 2015-01-12 2015-04-29 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
US20120136604A1 (en) * 2010-11-30 2012-05-31 Industrial Technology Research Institute Method and apparatus for 3d attitude estimation
CN103630139A (zh) * 2013-12-17 2014-03-12 哈尔滨工程大学 一种基于地磁梯度张量测量的水下载体全姿态确定方法
CN104567871A (zh) * 2015-01-12 2015-04-29 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
P. WU等: "Research on attitude updating algorithm based on modified Rodrigues parameters for low cost attitude and heading reference system", 《THE 2010 IEEE INTERNATIONAL CONFERENCE ON INFORMATION AND AUTOMATION》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109990776A (zh) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN110849391A (zh) * 2019-11-01 2020-02-28 上海卫星工程研究所 矢量磁力仪与星敏感器非正交安装矩阵标定系统及方法
CN110849391B (zh) * 2019-11-01 2021-07-13 上海卫星工程研究所 矢量磁力仪与星敏感器非正交安装矩阵标定系统及方法
CN111678515A (zh) * 2020-04-28 2020-09-18 北京三快在线科技有限公司 设备状态估计方法、装置、电子设备及存储介质
CN112033405A (zh) * 2020-08-10 2020-12-04 北京摩高科技有限公司 一种室内环境磁异常实时修正与导航方法及装置

Also Published As

Publication number Publication date
CN109000639B (zh) 2022-01-07

Similar Documents

Publication Publication Date Title
Storms Magnetic field aided indoor navigation
Le Grand et al. 3-axis magnetic field mapping and fusion for indoor localization
Fang et al. A novel calibration method of magnetic compass based on ellipsoid fitting
CN105180968B (zh) 一种imu/磁强计安装失准角在线滤波标定方法
Hong et al. Observability of error states in GPS/INS integration
CN109556632A (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN105371844B (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN107525503A (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN109000639A (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
US20040123474A1 (en) Methods and apparatus for automatic magnetic compensation
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
Wahdan et al. Magnetometer calibration for portable navigation devices in vehicles using a fast and autonomous technique
Allotta et al. Single axis FOG aided attitude estimation algorithm for mobile robots
CN105606094A (zh) 一种基于mems/gps组合系统的信息条件匹配滤波估计方法
CN107270898B (zh) 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法
Wahdan et al. Three-dimensional magnetometer calibration with small space coverage for pedestrians
CN109556631A (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
Shockley Ground vehicle navigation using magnetic field variation
CN103630139A (zh) 一种基于地磁梯度张量测量的水下载体全姿态确定方法
CN107024206A (zh) 一种基于ggi/gps/ins的组合导航系统
Liang et al. A solution to the attitude problem using two rotation units of micromechanical gyroscopes
CN110440746A (zh) 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法
Patel et al. Multi-IMU based alternate navigation frameworks: Performance & comparison for UAS

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