CN116817896A - 一种基于扩展卡尔曼滤波的姿态解算方法 - Google Patents
一种基于扩展卡尔曼滤波的姿态解算方法 Download PDFInfo
- Publication number
- CN116817896A CN116817896A CN202310343630.8A CN202310343630A CN116817896A CN 116817896 A CN116817896 A CN 116817896A CN 202310343630 A CN202310343630 A CN 202310343630A CN 116817896 A CN116817896 A CN 116817896A
- Authority
- CN
- China
- Prior art keywords
- magnetometer
- matrix
- vector
- state
- coordinate system
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 95
- 238000001914 filtration Methods 0.000 title claims abstract description 22
- 239000011159 matrix material Substances 0.000 claims abstract description 135
- 238000005259 measurement Methods 0.000 claims abstract description 61
- 238000013213 extrapolation Methods 0.000 claims abstract description 38
- 238000012937 correction Methods 0.000 claims abstract description 22
- 230000008569 process Effects 0.000 claims abstract description 12
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 238000010276 construction Methods 0.000 claims abstract description 4
- 239000013598 vector Substances 0.000 claims description 90
- 230000001133 acceleration Effects 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 16
- 230000014509 gene expression Effects 0.000 claims description 13
- 238000007476 Maximum Likelihood Methods 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 12
- 230000007704 transition Effects 0.000 claims description 12
- 238000012546 transfer Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 2
- 238000009434 installation Methods 0.000 abstract description 3
- XEEYBQQBJWHFJM-UHFFFAOYSA-N Iron Chemical compound [Fe] XEEYBQQBJWHFJM-UHFFFAOYSA-N 0.000 description 40
- 230000000694 effects Effects 0.000 description 20
- 229910052742 iron Inorganic materials 0.000 description 20
- 230000008878 coupling Effects 0.000 description 8
- 238000010168 coupling process Methods 0.000 description 8
- 238000005859 coupling reaction Methods 0.000 description 8
- 230000005415 magnetization Effects 0.000 description 4
- 239000000463 material Substances 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 239000000696 magnetic material Substances 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000000758 substrate Substances 0.000 description 2
- 125000003275 alpha amino acid group Chemical group 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/38—Testing, calibrating, or compensating of compasses
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01R—MEASURING ELECTRIC VARIABLES; MEASURING MAGNETIC VARIABLES
- G01R33/00—Arrangements or instruments for measuring magnetic variables
- G01R33/0023—Electronic aspects, e.g. circuits for stimulation, evaluation, control; Treating the measured signals; calibration
- G01R33/0035—Calibration of single magnetic sensors, e.g. integrated calibration
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Condensed Matter Physics & Semiconductors (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于扩展卡尔曼滤波的姿态解算方法,包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块,加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块;本发明属于导航技术领域,尤其涉及惯性导航领域,具体是指本发明提供一种基于扩展卡尔曼滤波的姿态解算方法,本发明的优点为:可将磁强计误差和畸变矫正与姿态解算方法同时进行,考虑了传感器误差和由磁强计安装引起的磁扰动,并考虑到了与惯性传感器的轴对齐问题,进而使位姿估计更加准确;使用扩展卡尔曼滤波,使估计结果越来越可靠;将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵,在满足实时性要求的同时获得精度较高的结果。
Description
技术领域
本发明属于导航技术领域,尤其涉及惯性导航领域,具体为一种基于扩展卡尔曼滤波的姿态解算方法。
背景技术
捷联式惯性导航系统 (Strapdown inertial navigation system)是把惯性器件直接固连在载体上,用计算机来完成导航平台功能的惯性导航系统;其有点包括:1.整个系统的体积、重量和成本大大降低;2.惯性仪表便于安装维护,便于更换;3.惯性器件可以给出轴向的线加速度和角速度,这些信息是控制系统所需要的。和平台式系统相比,捷联式系统可以提供更多的导航和制导信息;4.惯性器件便于采用余度配置,提高系统的性能和可靠性。
三轴陀螺仪和三轴加速度计(惯性传感器)与三轴磁强计组成9轴惯性单元,9轴惯性单元与载体“捆绑”,载体的姿态和航向可以用载体坐标系(b系)相对于导航坐标系(n系)的三个转动角决定,就是说,在捷联式惯性导航系统中,直接测得的三个方向的加速度是载体坐标系三个轴方向上的,需要用一个捷联矩阵C将其变换到导航坐标系中来。惯性传感器安装在载体;该形式的捷联式惯性导航系统的任务是解算出载体相对于当地水平坐标系的位姿,速度和位置,其中,最为关键的任务是位姿的解算。
三轴磁力计、三轴加速度计和三轴陀螺仪的测量值可以结合起来,给出三维的方向估计。为了获得准确的方向估计,必须对磁强计和惯性传感器(加速度计和陀螺仪)进行轴对齐,并对磁强计的传感器误差和磁畸变进行校正。
发明内容
(一)要解决的技术问题
为解决现有技术的上述问题,本发明提供一种基于扩展卡尔曼滤波的姿态解算方法,可以有效解决:
(1)传统的姿态估计方法需要针对不同的传感器类型将磁强计误差和畸变矫正与姿态解算方法分开进行,增加了算法部署的步骤,另一方面,传统的磁强计的校准磁强计只考虑了传感器误差和由磁强计安装引起的磁扰动,没有考虑到与惯性传感器的轴对齐问题,进而造成位姿估计不准确;
(2)传统基于四元数的位姿估计由于惯性测量单元的特点使误差逐渐增大,使估计结果越来越不可靠;
(3)传统使用扩展卡尔曼滤波的姿态估计方法需实时计算过程噪声矩阵使计算量增大,对于实时性要求较高的应用场景,该方法就显得差强人意,基于传统四元数的扩展卡尔曼姿态估计在实践中的表现并不理想。
(二)技术方案
为了解决上述问题,本发明采用的一种技术方案为:一种基于扩展卡尔曼滤波的姿态解算方法,包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块,加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块;在线磁强计校正模块的作用是消除原始磁强计数据的零偏误差、交轴耦合误差和比例误差,并将磁强计的三个轴与加速度计三个轴进行轴向对齐,为实现此功能,在线磁强计校正模块的操作步骤如下:
S1、建立磁强计测量模型:
一、设使用磁强计进行测量的测量数据集为:,上标b表示磁强计的传感器坐标系(也称body系),k表示第k个采样数据,上标N表示数据集中的样本总数,每个样本包含三个分量,分别是磁强计X轴的读数,磁强计Y轴的读数和磁强计Z轴的读数;当地磁场矢量从n系(当地水平坐标系)到b系(传感器坐标系)的转化用旋转矩阵用表示,对第k个采样数据,n系的当地磁场转换到b系中来的公式为:
上标n表示导航坐标系(n系也叫当地水平坐标系,采用北东地NED)
进而有:
上式是对每一个样本点根据当地磁场和对应的旋转矩阵用估计的结果,我们希望实际测量值与估计值相等,也就是成立,然而,有两个原因导致它们在实践中通常不平等,需要校准;磁力计三个轴的传感器误差:每个磁力计的轴向误差不同。这些传感器误差包括:
(1)磁强计三个轴的交轴耦合,使用3×3的矩阵N表示磁强计三个轴的交轴耦合;
(2)磁强计零偏置,即使磁场为零,磁强计测量结果为非零,零偏置由3×1向量b定义;
(3)磁强计三个轴的比例因子不同,用3×3对角矩阵S m表示;
(4)在磁强计的测量中存在高斯噪声,用3×1向量表示;
这导致了和之间的以下关系,只考虑传感器误差:
二、考虑硬铁效应和软铁效应:硬铁效应是由于磁性材料的永久磁化强度,会导致一个恒定的附加磁场,表示这个效应的向量记为h;软铁效应是由于外部磁场对材料的磁化作用,因此将取决于材料相对于外部磁场的方向,它可以改变被测磁场的大小和方向,表示软铁效应的3×3矩阵记为S i ;假设软铁产生的场与外部磁场之间存在线性关系,这是一个合理的假设,因为磁滞只有在施加非常大的磁场时才会发生;在考虑了硬铁效应和软铁效应后,传感器误差模型变为:
假设满足这些要求,式(1)意味着n系中的投影到b系中的将位于一个半径等于局部磁场大小的球体上。对式(3),假设所有的样本的数据应该位于一个椭球体上,这个椭球体可以相对于上述球体进行平移、旋转和缩放。
三、考虑磁力计和惯性传感器轴不对齐:现在考虑磁强计三轴与惯性传感器的轴不对齐问题造成位姿估计不准确,使用旋转矩阵R im 描述惯性传感器和磁力计之间的错位关系,式(4)变为:
其中:
最终的校正模型为:
上式中,D为失真矩阵,q为偏移向量,意味着为了获得正确的校准,不需要在式(5)中区分交轴耦合、零偏置、比例因子、硬铁效应、软铁效应和磁力计和惯性传感器轴不对齐,它们可以组合成一个3×3失真矩阵D和一个3×1偏移向量q;
S2、最大似然建模:
一、磁强计的原始数据通过将椭球体数据映射到球体来补偿传感器误差和磁畸变的存在;然而,磁力计和惯性传感器轴不对齐不能仅仅通过磁强计的测量来校正;将磁强计测量和惯性测量相结合以确定这种旋转是必要的,椭球的旋转可以通过使用磁强计测量和处理过的垂向参考值来确定,其中第k个样本表示为:
其中:
式(8)中,服从高斯分布,z n 为单位垂向向量,处理后的测量值可以通过保持加速度计在不同方向上的静止来得到,且模长也为1;
二、当地地球磁场建模:
当地地球磁场由一个水平分量和一个垂直分量组成,设地球磁场与水平面成δ角,由sin δ定义的垂直分量,由cos δ定义的水平分量,
上式中,表示的模长,和水平面之间的夹角δ取决于在地球上的位置,d表示垂向分量,由于公式(7)中的校准结果D和q只依赖于水平分量和垂直分量之间的比例,因此可以假设为零;
从公式(10)和的定义中可以看出,和的内积为的垂直分量d;因此,和的内积对于所有可能的旋转上也等于d;然而,当惯性和磁力计传感器轴不对齐时,基于磁力计和垂直测量的每个的估计d将会有所不同,选择一种旋转R im 可使惯性和磁强计传感器轴对齐,而使估计的倾角d近似恒定;
S3、构建最大似然公式:磁强计测量值在校准前位于一个椭球体上,并在校准后位于一个半径为||||的球体上,将球体的半径缩放为1,即||mn || = 1;
结合式(7)和式(1)得:
定义参数θ 1为:
如前所述,上式中,D表示失真矩阵,q表示偏移向量,是第二个分量为0的当地磁场矢量,表示所有样本点从n系到b系的旋转矩阵;假设噪声是独立的和高斯分布的,有:
其中和为对角矩阵:
定义参数θ 2为:
S4、求解极大似然问题,也就是使和尽可能相等,从而找到未知参数θ 1和θ 2的估计值,有:
Y 1:k定义为:
上式中,上标m表示估计值,上标z表示测量值,由于噪声服从独立的高斯分布,则将ML问题简化为:
使用三个分量参数化旋转矩阵和mn时,假设Σ m和Σ z是对角矩阵,求解给定θ 1的θ 2是通过先求解假设θ 2为恒等式的θ 1,然后求解给定θ 1的θ 2,最后求解给定θ 2的θ 1,而不是一次性求解式(14)。
进一步地,所述状态变量确定模块的处理步骤如下:
S1、定义状态变量:
上式中,q 0 是四元数的实部,q 1 ,q 2 ,q 3 分别是四元数虚部的三个分量;b 1 ,b 2 ,b 3 分别是陀螺仪在x,y,z 三个方向的偏置;偏置指的是陀螺仪在单位时间内的漂移量,可将式(15)写成如下形式:
上式中,q表示姿态四元数,b表示偏置向量。
S2、定义四元数求导法则:
其中:S(ω)、q、S(q)、ω的表达式分别为:
S3、为了补偿陀螺仪的偏差,在方程中加入偏差项:
S4、使用一阶线性化模型将上述方程进行离散化:
上式中,T是k时刻到k+1时刻的采样时间,q(k)是k时刻的姿态四元数,q(k+1)是k+1时刻的姿态四元数,是k时刻的姿态四元数关于时间的导数;
将式(18)代入式(19)中得:
因此,系统状态方程可以写成:
也可以写成:
上式可简写为:
矩阵A为状态转移矩阵,矩阵B为输入转移矩阵,为k时刻的状态变量,为k时刻的输入向量,为外推的状态向量;上式就是扩展卡尔曼滤波中的状态外推方程。
进一步地,所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤:
S1、首先其中加速度估计将重力矢量作为参考矢量,重力矢量总是指向当地水平坐标系的下方,有:
上式中,是加速度计在传感器坐标系(b系)下估计的测量加速度;是当地水平坐标系到传感器坐标系的旋转矩阵;g是当地水平坐标系下的重力向量,g = [0 0 1]T;是传感器坐标系下的加速度计零偏,当对加速度计做了校正后是0;因为可以确定等号右边的所有变量,因此可以估计加速度计的测量值,然后使用加速度计的预测测量值与实际测量值进行比较,以确定方向误差,从单位四元数到旋转矩阵的定义如下:
由于重力向量为g = [0 0 1]T;因此有:
上式中,含有四元数的矩阵是非线性的,因此,为使用卡尔曼滤波,将上式进行线性化,并写成形如的形式,是状态向量,是余项,扩展卡尔曼滤波的处理方式是将上式进行线性化,根据泰勒展开,有:
其中为雅可比矩阵:
将上式写成的形式,C a称为线性系数矩阵,并忽略矩阵有:
S2、其次,磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量,这个矢量在地球表面上的变化(矢量的高度取决于海拔) ,假设北参考矢量的变化是可以忽略不计的,因为传感器不会移动太多(只在房间内);假设磁强计数据在水平面上提供的参考矢量是准确的,因此要从磁强计参考矢量中去掉1维;
上式中,h表示真实的磁场,h m表示磁强计测得的磁场;D -1表示失真矩阵的逆矩阵,q是偏移向量,都是在线磁强计校正模块的处理结果;接下来,为了从磁强计矢量中移除1维(垂直平面) ,由于想移除的磁场垂直分量存在于当地水平坐标系中,因此首先将坐标从传感器坐标系转换为当地水平坐标系,因此,由四元数生成旋转矩阵,如下所示:
上式中,
,
上式中,r的每个分量是传感器坐标系下校正之后的磁强计数据,r’的每个分量是在当地水平坐标系下校正之后的磁强计数据,令r’的第二个分量等于零,然后重新标准化向量,使它保持为一个单位向量,然后将旋转它回到传感器坐标系,并使用结果向量代替实际校准的测量数据。
使用北矢量作为磁强计的参考,因为北矢量恰好指向x轴的正方向,所以只是把正x轴(当地水平坐标系)作为参考矢量;
其中,是磁强计在传感器坐标系下的估计的磁场强度值,是当地水平坐标系到传感器坐标系的旋转矩阵,是当地水平坐标系下参考磁场的北向分量,且有=[1 00 ]T;是传感器坐标系下的磁强计零偏值,当对磁强计进行校正后,为零,旋转矩阵的表达式为:
化简得:
上式依然是一个非线性方程,利用泰勒展开可以化简为:
因此,线性化之后的表达式为:
进一步地,所述姿态偏置状态外推模块的处理过程如下:
S1、进行状态外推:;在状态外推中,表示由k-1时刻的四元数生成的状态转移矩阵、是k-1时刻的状态变量、表示由k-1时刻的四元数生成的输入转移矩阵、为k时刻输入,也就是k时刻的陀螺仪的测量值,是外推结果,也就是外推得到的状态变量;
S2、进行协方差外推:;在协方差外推中,Q为过程噪声协方差矩阵,是k - 1时刻的状态误差协方差矩阵,表示由k-1时刻的四元数生成的状态转移矩阵;表示外推得到的k时刻的状态误差协方差矩阵。
进一步地,所述姿态偏置状态更新模块的处理过程如下:
S1、计算卡尔曼增益矩阵:
上式中R是测量协方差矩阵,C是线性系数矩阵(也称为测量矩阵),表示外推得到的k时刻的状态误差协方差矩阵,K k 为卡尔曼增益矩阵;
S2、进行状态更新:
上式中是外推得到的状态变量,C是线性系数矩阵(也称为测量矩阵),K k 为卡尔曼增益矩阵,是k时刻的加速度计测量和磁强计测量的组合向量,是更新得到的状态变量;
S3、进行协方差更新:
上式中,I是7×7的单位矩阵,K k 为卡尔曼增益矩阵,表示外推得到的k时刻的状态误差协方差矩阵,C是线性系数矩阵,R k 是测量误差协方差矩阵,是更新后的状态误差协方差矩阵。
(三)有益效果
本发明提供一种基于扩展卡尔曼滤波的姿态解算方法,可有效解决:
(1)将磁强计误差和畸变矫正与姿态解算方法同时进行,考虑了传感器误差和由磁强计安装引起的磁扰动,并考虑到了与惯性传感器的轴对齐问题,进而使位姿估计更加准确;
(2)使用扩展卡尔曼滤波,使估计结果越来越可靠;
(3)将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵,在满足实时性要求的同时获得精度较高的结果。
附图说明
图1为本发明了一种基于扩展卡尔曼滤波的姿态解算方法的处理流程图;
图2为本发明提出的在线磁强计校正模块的处理流程图;
图3为本发明提出的加速度计与磁强计估计模块的加速度线性估计表达式和磁场计算表达式;
图4为本发明提出的姿态偏置状态外推模块的状态外推表达式和协方差外推表达式;
图5为本发明提出的姿态偏置状态更新模块的卡尔曼增益计算、状态更新和协方差更新的计算步骤。
附图用来提供对本发明的进一步理解,并且构成说明书的一部分,与本发明的实施例一起用于解释本发明,并不构成对本发明的限制。
具体实施方式
下面将结合本方案实施例中的附图,对本方案实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本方案一部分实施例,而不是全部的实施例;基于本方案中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本方案保护的范围。
一种基于扩展卡尔曼滤波的姿态解算方法,包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块、加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块;在线磁强计校正模块的作用是消除原始磁强计数据的零偏误差、交轴耦合误差和比例误差,并将磁强计的三个轴与加速度计三个轴进行轴向对齐,为实现此功能,在线磁强计校正模块的操作步骤如下:
S1、建立磁强计测量模型:
一、设使用磁强计进行测量的测量数据集为:,上标b表示磁强计的传感器坐标系(也称body系),k表示第k个采样数据,上标N表示数据集中的样本总数,每个样本包含三个分量,分别是磁强计X轴的读数,磁强计Y轴的读数和磁强计Z轴的读数;当地磁场矢量从n系(当地水平坐标系)到b系(传感器坐标系)的转化用旋转矩阵用表示,对第k个采样数据,n系的当地磁场转换到b系中来的公式为:
上标n表示导航坐标系(n系也叫当地水平坐标系,采用北东地NED)
进而有:
上式是对每一个样本点根据当地磁场和对应的旋转矩阵用估计的结果,我们希望实际测量值与估计值相等,也就是成立,然而,有两个原因导致它们在实践中通常不平等,需要校准;磁力计三个轴的传感器误差:每个磁力计的轴向误差不同。这些传感器误差包括:
(1)磁强计三个轴的交轴耦合,使用3×3的矩阵N表示磁强计三个轴的交轴耦合;
(2)磁强计零偏置,即使磁场为零,磁强计测量结果为非零,零偏置由3×1向量b定义;
(3)磁强计三个轴的比例因子不同,用3×3对角矩阵S m表示;
(4)在磁强计的测量中存在高斯噪声,用3×1向量表示;
这导致了和之间的以下关系,只考虑传感器误差:
二、考虑硬铁效应和软铁效应:硬铁效应是由于磁性材料的永久磁化强度,会导致一个恒定的附加磁场,表示这个效应的向量记为h;软铁效应是由于外部磁场对材料的磁化作用,因此将取决于材料相对于外部磁场的方向,它可以改变被测磁场的大小和方向,表示软铁效应的3×3矩阵记为S i ;假设软铁产生的场与外部磁场之间存在线性关系,这是一个合理的假设,因为磁滞只有在施加非常大的磁场时才会发生;在考虑了硬铁效应和软铁效应后,传感器误差模型变为:
假设满足这些要求,式(1)意味着n系中的投影到b系中的将位于一个半径等于局部磁场大小的球体上。对式(3),假设所有的样本的数据应该位于一个椭球体上,这个椭球体可以相对于上述球体进行平移、旋转和缩放。
三、考虑磁力计和惯性传感器轴不对齐:现在考虑磁强计三轴与惯性传感器的轴不对齐问题造成位姿估计不准确,使用旋转矩阵R im 描述惯性传感器和磁力计之间的错位关系,式(4)变为:
其中:
最终的校正模型为:
上式中,D为失真矩阵,q为偏移向量,意味着为了获得正确的校准,不需要在式(5)中区分交轴耦合、零偏置、比例因子、硬铁效应、软铁效应和磁力计和惯性传感器轴不对齐,它们可以组合成一个3×3失真矩阵D和一个3×1偏移向量q;
S2、最大似然建模:
一、磁强计的原始数据通过将椭球体数据映射到球体来补偿传感器误差和磁畸变的存在;然而,磁力计和惯性传感器轴不对齐不能仅仅通过磁强计的测量来校正;将磁强计测量和惯性测量相结合以确定这种旋转是必要的,椭球的旋转可以通过使用磁强计测量和处理过的垂向参考值来确定,其中第k个样本表示为:
其中:
式8中,服从高斯分布,z n 为单位垂向向量,处理后的测量值可以通过保持加速度计在不同方向上的静止来得到,且模长也为1;
二、当地地球磁场建模:
当地地球磁场由一个水平分量和一个垂直分量组成,设地球磁场与水平面成δ角,由sin δ定义的垂直分量,由cos δ定义的水平分量,
上式中,表示的模长,和水平面之间的夹角δ取决于在地球上的位置,d表示垂向分量,由于公式(7)中的校准结果D和q只依赖于水平分量和垂直分量之间的比例,因此可以假设为零;
从公式(10)和的定义中可以看出,和的内积为的垂直分量d;因此,和的内积对于所有可能的旋转上也等于d;然而,当惯性和磁力计传感器轴不对齐时,基于磁力计和垂直测量的每个的估计d将会有所不同,选择一种旋转R im 可使惯性和磁强计传感器轴对齐,而使估计的倾角d近似恒定;
S3、构建最大似然公式:磁强计测量值在校准前位于一个椭球体上,并在校准后位于一个半径为||||的球体上,将球体的半径缩放为1,即||mn || = 1;
结合式(7)和式(1)得:
定义参数θ 1为:
如前所述,上式中,D表示失真矩阵,q表示偏移向量,是第二个分量为0的当地磁场矢量,表示所有样本点从n系到b系的旋转矩阵;假设噪声是独立的和高斯分布的,有:
其中和为对角矩阵:
定义参数θ 2为:
S4、求解极大似然问题,也就是使和尽可能相等,从而找到未知参数θ 1和θ 2的估计值,有:
Y 1:k定义为:
上式中,上标m表示估计值,上标z表示测量值,由于噪声服从独立的高斯分布,则将ML问题简化为:
使用三个分量参数化旋转矩阵和mn时,假设Σ m和Σ z是对角矩阵,求解给定θ 1的θ 2是通过先求解假设θ 2为恒等式的θ 1,然后求解给定θ 1的θ 2,最后求解给定θ 2的θ 1,而不是一次性求解式(14)。
进一步地,所述状态变量确定模块的处理步骤如下:
S1、定义状态变量:
上式中,q 0 是四元数的实部,q 1 ,q 2 ,q 3 分别是四元数虚部的三个分量;b 1 ,b 2 ,b 3 分别是陀螺仪在x,y,z 三个方向的偏置;偏置指的是陀螺仪在单位时间内的漂移量,可将式(15)写成如下形式:
上式中,q表示姿态四元数,b表示偏置向量。
S2、定义四元数求导法则:
其中:S(ω)、q、S(q)、ω的表达式分别为:
S3、补偿陀螺仪的偏差,在方程中加入偏差项:
S4、使用一阶线性化模型将上述方程进行离散化:
上式中,T是k时刻到k+1时刻的采样时间,q(k)是k时刻的姿态四元数,q(k+1)是k+1时刻的姿态四元数。
将式(18)代入式(19)中得:
式(20)也可以表达为:
所述状态方程构建模块的计算步骤如下:
S1、结合方程(15)、(18)和(21)系统状态方程可以写成:
式(22)也可以写成:
式(23)的展开形式为:
S2、将方程(24)化简为:
矩阵A为状态转移矩阵,矩阵B为输入转移矩阵,为k时刻的状态变量,为k时刻的输入向量,为外推的状态向量;上式就是扩展卡尔曼滤波中的状态外推方程。
所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤:
S1、加速度估计,将重力矢量作为参考矢量,重力矢量总是指向当地水平坐标系的下方,有:
上式中,是加速度计在传感器坐标系(b系)测量的加速度;是当地水平坐标系到传感器坐标系的旋转矩阵;g是当地水平坐标系下的重力向量,g = [0 0 1]T;是传感器坐标系下的加速度计零偏,当对加速度计做了校正后是0;因为可以确定等号右边的所有变量 ,因此可以预测加速度计的测量值,然后使用加速度计的预测测量值与实际测量值进行比较,以确定方向误差,从单位四元数到旋转矩阵的定义如下:
由于重力向量为g = [0 0 1]T;因此有:
整理有:
上式中,含有四元数的矩阵是非线性的,因此,为使用卡尔曼滤波,将上式进行线性化,并写成形如的形式,是状态向量,是余项,也就是预测的加速度计的测量值,扩展卡尔曼滤波的处理方式是将上式进行线性化:
根据泰勒展开,有:
其中为雅可比矩阵,表达式为:
将上式写成的形式,C a称为加速度线性系数矩阵,并忽略矩阵有:
上式中,令:
S2、磁场估计,磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量,这个矢量在地球表面上的变化 ,假设北参考矢量的变化是可以忽略不计的,因为传感器不会移动太多;磁强计数据在水平面上提供的参考矢量是准确的;因此要从磁强计参考矢量中去掉1维;
上式中,h表示真实的磁场,h m表示磁强计测得的磁场;D -1表示失真矩阵的逆矩阵,q是偏移向量,都是在线磁强计校正模块的处理结果。
接下来,为了从磁强计矢量中移除1维,由于想移除的磁场垂直分量存在于当地水平坐标系中,因此首先将坐标从传感器坐标系转换为当地水平坐标系,因此,由四元数生成旋转矩阵,如下所示:
上式中,
,
上式中,r的每个分量是传感器坐标系下校正之后的磁强计数据,r’的每个分量是在当地水平坐标系下校正之后的磁强计数据,令r’的第二个分量等于零,然后重新标准化向量,使它保持为一个单位向量,然后将旋转它回到传感器坐标系,并使用结果向量代替实际校准的测量数据。
在加速度计部分,重力矢量作为参考矢量;另一方面,北矢量作为磁强计的参考;因为北矢量恰好指向x轴的正方向,所以只是把正x轴(当地水平坐标系)作为参考矢量;
其中,是磁强计在传感器坐标系下的测量的磁场强度值,是当地水平坐标系到传感器坐标系的旋转矩阵,是当地水平坐标系下参考磁场的北向分量,且有=[1 00 ]T;是传感器坐标系下的磁强计零偏值,当对磁强计进行校正后,为零;
化简得:
上式依然是一个非线性方程,利用泰勒展开可以化简为:
其中为雅可比矩阵,表达式为:
进一步有:
进一步有:
因此,线性化之后得表达式为:
上式中,令:
其中,C m称为磁场线性系数矩阵。
所述姿态偏置状态外推模块的处理过程如下:
S1、进行状态外推:;在状态外推中,表示由k-1时刻的四元数生成的状态转移矩阵、是k-1时刻的状态变量、表示由k-1时刻的四元数生成的输入转移矩阵、为k时刻输入,也就是k时刻的陀螺仪的测量值,是外推结果,也就是外推得到的状态变量;
S2、进行协方差外推:;在协方差外推中,Q为过程噪声协方差矩阵,是k-1时刻的状态误差协方差矩阵,表示由k-1时刻的四元数生成的状态转移矩阵;表示外推得到的k时刻的状态误差协方差矩阵。
所述姿态偏置状态更新模块的处理过程如下:
S1、计算卡尔曼增益矩阵:;其中R是测量误差协方差矩阵,C是线性系数矩阵(也称为测量矩阵),表示外推得到的k时刻的状态误差协方差矩阵,K k为卡尔曼增益矩阵;测量矩阵C的表达式如下定义:
S2、进行状态更新:;其中是外推得到的状态变量,C是线性系数矩阵(也称为测量矩阵),K k为卡尔曼增益矩阵,是k时刻的加速度计测量和磁强计测量的组合向量,是更新得到的状态变量;
S3、进行协方差更新:;其中,I是7×7的单位矩阵,K k为卡尔曼增益矩阵,表示外推得到的k时刻的状态误差协方差矩阵,C是线性系数矩阵,R k是测量误差协方差矩阵,是更新后的状态误差协方差矩阵。
实施例一:
一种基于扩展卡尔曼滤波的姿态解算方法,包括在线磁强计校正模块、加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块;实施方式如下:
S1、使用在线磁强计校正模块进行在线磁强计校正,操作步骤如下:
(1)、建立磁强计测量模型;
(2)、最大似然建模:
(3)、构建最大似然公式:
(4)、求解极大似然问题;
S2、使用状态变量确定模块的确定状态变量:
(1)、定义状态变量;
(2)、定义四元数求导法则:
S3、使用状态方程构建模块构建状态方程:
S4、使用加速度计与磁力计估计模块估计加速度和磁场:
(1)、加速度估计,最终结果为:
上式中,令:
其中,C a称为加速度线性系数矩阵。
(2)、磁场估计,最终结果为:
上式中,令:
其中,C m称为磁场线性系数矩阵。
S5、使用姿态偏置状态外推模块进行状态外推和协方差外推,步骤如下:
(1)、进行状态外推:;在状态外推中,表示由k-1时刻的四元数生成的状态转移矩阵、是k-1时刻的状态变量、表示由k-1时刻的四元数生成的输入转移矩阵、为k时刻输入,也就是k时刻的陀螺仪的测量值,是外推结果,也就是外推得到的状态变量;
(2)、进行协方差外推:;在协方差外推中,Q为过程噪声协方差矩阵,是k-1时刻的状态误差协方差矩阵,表示由k-1时刻的四元数生成的状态转移矩阵;表示外推得到的k时刻的状态误差协方差矩阵。
S6、使用姿态偏置状态更新模块进行状态更新和协方差更新,步骤如下:
(1)、计算卡尔曼增益矩阵:;
(2)、进行状态更新:;
(3)、进行协方差更新:;
S7、步骤S4、S5和S6循环进行。
以上便是本发明具体的工作流程,下次使用时重复此步骤即可。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例,对于本领域的普通技术人员而言,可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型,本发明的范围由所附权利要求及其等同物限定。
以上对本发明及其实施方式进行了描述,这种描述没有限制性,附图中所示的也只是本发明的实施方式之一,实际的结构并不局限于此。总而言之如果本领域的普通技术人员受其启示,在不脱离本发明创造宗旨的情况下,不经创造性的设计出与该技术方案相似的结构方式及实施例,均应属于本发明的保护范围。
Claims (6)
1.一种基于扩展卡尔曼滤波的姿态解算方法,包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块,加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块;在线磁强计校正模块的操作步骤如下:
S1、建立磁强计测量模型:
S2、最大似然建模:
S3、构建最大似然公式:
S4、求解极大似然问题。
2.根据权利要求1所述的一种基于扩展卡尔曼滤波的姿态解算方法,其特征在于:所述状态变量确定模块的处理步骤如下:
S1、定义状态变量:
,
上式中,q 0 是四元数的实部,q 1 ,q 2 ,q 3 分别是四元数虚部的三个分量;b 1 ,b 2 ,b 3 分别是陀螺仪在x,y,z 三个方向的偏置;偏置指的是陀螺仪在单位时间内的漂移量,可将式(15)写成如下形式:
,
上式中,q表示姿态四元数,b表示由b 1 ,b 2 和b 3 组成的偏置向量;
S2、定义四元数求导法则:
,
其中:S(q)、ω的表达式分别为:
,
上式中,ω表示陀螺仪在x轴、y轴和z轴三个方向测量的角速度组成的角速度向量;
S3、补偿陀螺仪的偏差,在方程中加入偏差项;
,
进一步将公式(18)化简为:
,
上式中,ω表示陀螺仪在x轴、y轴和z轴三个方向测量的角速度值,b表示陀螺仪在x,y,z 三个方向的偏差b 1 ,b 2 和b 3 组成的偏差向量;
S4、使用一阶线性化模型将上述方程进行离散化:
,
上式中,T是k时刻到k+1时刻的采样时间,q(k)是k时刻的姿态四元数,q(k+1)是k+1时刻的姿态四元数,是k时刻的姿态四元数关于时间的导数,进一步有:
,
将式(18)代入式(19)中得:
,
进一步有:
。
3.根据权利要求2所述的一种基于扩展卡尔曼滤波的姿态解算方法,其特征在于:所述状态方程构建模块的计算步骤如下:
S1、结合方程(15)、(18)和(21)的系统状态方程可以写成:
,
因此,系统状态方程可以写成:
,
S2、将方程(23)化简为:
,
矩阵A为状态转移矩阵,矩阵B为输入转移矩阵,为k时刻的状态变量,为k时刻的输入向量,为外推的状态向量;上式就是扩展卡尔曼滤波中的状态外推方程。
4.根据权利要求3所述的一种基于扩展卡尔曼滤波的姿态解算方法,其特征在于:所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤:
S1、加速度估计,加速度估计的线性化结果为:首先其中加速度估计将重力矢量作为参考矢量,重力矢量总是指向当地水平坐标系的下方,有:
,
上式中,是加速度计在传感器坐标系(b系)下估计的测量加速度;是当地水平坐标系到传感器坐标系的旋转矩阵;g是当地水平坐标系下的重力向量,g = [0 0 1]T;是传感器坐标系下的加速度计零偏,当对加速度计做了校正后是0;因为可以确定等号右边的所有变量,因此可以估计加速度计的测量值,然后使用加速度计的预测测量值与实际测量值进行比较,以确定方向误差,
,
S2、其次,磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量,这个矢量在地球表面上的变化(矢量的高度取决于海拔) ,假设北参考矢量的变化是可以忽略不计的,因为传感器不会移动太多(只在房间内);假设磁强计数据在水平面上提供的参考矢量是准确的,因此要从磁强计参考矢量中去掉1维;
,
上式中,h表示真实的磁场,h m 表示磁强计测得的磁场;D -1表示失真矩阵的逆矩阵,q是偏移向量,都是在线磁强计校正模块的处理结果;接下来,为了从磁强计矢量中移除1维(垂直平面) ,由于想移除的磁场垂直分量存在于当地水平坐标系中,因此首先将坐标从传感器坐标系转换为当地水平坐标系,因此,由四元数生成旋转矩阵,如下所示:
,
上式中,
, ,
上式中,r的每个分量是传感器坐标系下校正之后的磁强计数据,r'的每个分量是在当地水平坐标系下校正之后的磁强计数据,令r'的第二个分量等于零,然后重新标准化向量,使它保持为一个单位向量,然后将旋转它回到传感器坐标系,并使用结果向量代替实际校准的测量数据;
使用北矢量作为磁强计的参考,因为北矢量恰好指向x轴的正方向,所以只是把正x轴(当地水平坐标系)作为参考矢量;
,
其中,是磁强计在传感器坐标系下的估计的磁场强度值,是当地水平坐标系到传感器坐标系的旋转矩阵,是当地水平坐标系下参考磁场的北向分量,且有=[1 0 0 ]T;是传感器坐标系下的磁强计零偏值,当对磁强计进行校正后,为零,旋转矩阵的表达式为:
,
化简得:
,
上式依然是一个非线性方程,利用泰勒展开可以化简为:
,
因此,线性化之后的表达式为:
。
5.根据权利要求4所述的一种基于扩展卡尔曼滤波的姿态解算方法,其特征在于:所述姿态偏置状态外推模块的处理过程如下:
S1、进行状态外推:;在状态外推中,表示由k-1时刻的四元数生成的状态转移矩阵、是k-1时刻的状态变量、表示由k-1时刻的四元数生成的输入转移矩阵、为k时刻输入,也就是k时刻的陀螺仪的测量值,是外推结果,也就是外推得到的状态变量;
S2、进行协方差外推:;在协方差外推中,Q为过程噪声协方差矩阵,是k-1时刻的状态误差协方差矩阵,表示由k-1时刻的四元数生成的状态转移矩阵;表示外推得到的k时刻的状态误差协方差矩阵。
6.根据权利要求5所述的一种基于扩展卡尔曼滤波的姿态解算方法,其特征在于:所述姿态偏置状态更新模块的处理过程如下:
S1、计算卡尔曼增益矩阵:
上式中R是测量协方差矩阵,C是线性系数矩阵(也称为测量矩阵),表示外推得到的k时刻的状态误差协方差矩阵,K k 为卡尔曼增益矩阵;
S2、进行状态更新:
上式中是外推得到的状态变量,C是线性系数矩阵(也称为测量矩阵),K k 为卡尔曼增益矩阵,是k时刻的加速度计测量和磁强计测量的组合向量,是更新得到的状态变量;
S3、进行协方差更新:
上式中,I是7×7的单位矩阵,K k 为卡尔曼增益矩阵,表示外推得到的k时刻的状态误差协方差矩阵,C是线性系数矩阵,R k 是测量误差协方差矩阵,是更新后的状态误差协方差矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310343630.8A CN116817896B (zh) | 2023-04-03 | 2023-04-03 | 一种基于扩展卡尔曼滤波的姿态解算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310343630.8A CN116817896B (zh) | 2023-04-03 | 2023-04-03 | 一种基于扩展卡尔曼滤波的姿态解算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116817896A true CN116817896A (zh) | 2023-09-29 |
CN116817896B CN116817896B (zh) | 2024-04-16 |
Family
ID=88128180
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310343630.8A Active CN116817896B (zh) | 2023-04-03 | 2023-04-03 | 一种基于扩展卡尔曼滤波的姿态解算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116817896B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117289322A (zh) * | 2023-11-24 | 2023-12-26 | 江苏领创星通卫星通信科技有限公司 | 一种基于imu、gps与uwb的高精度定位算法 |
CN117419745A (zh) * | 2023-10-13 | 2024-01-19 | 南京理工大学 | 一种基于循环ekf的惯性辅助地磁在线标定方法及系统 |
CN118070576A (zh) * | 2024-04-24 | 2024-05-24 | 北京益普希环境咨询顾问有限公司 | 一种地下水污染源溯源辨识方法和系统 |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130185018A1 (en) * | 2010-10-01 | 2013-07-18 | Hua Sheng | Apparatuses and Methods for Estimating the Yaw Angle of a Device in a Gravitational Reference System Using Measurements of Motion Sensors and a Magnetometer Attached to the Device |
CN103630137A (zh) * | 2013-12-02 | 2014-03-12 | 东南大学 | 一种用于导航系统的姿态及航向角的校正方法 |
US20150177020A1 (en) * | 2012-08-02 | 2015-06-25 | Memsic, Inc. | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer |
CN105180968A (zh) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | 一种imu/磁强计安装失准角在线滤波标定方法 |
CN106643737A (zh) * | 2017-02-07 | 2017-05-10 | 大连大学 | 风力干扰环境下四旋翼飞行器姿态解算方法 |
CN107389092A (zh) * | 2017-06-27 | 2017-11-24 | 上海交通大学 | 一种基于磁传感器辅助的陀螺标定方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
CN110174121A (zh) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | 一种基于地磁场自适应修正的航姿系统姿态解算方法 |
CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
CN111551175A (zh) * | 2020-05-27 | 2020-08-18 | 北京计算机技术及应用研究所 | 一种航姿参考系统的互补滤波姿态解算方法 |
CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法 |
-
2023
- 2023-04-03 CN CN202310343630.8A patent/CN116817896B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130185018A1 (en) * | 2010-10-01 | 2013-07-18 | Hua Sheng | Apparatuses and Methods for Estimating the Yaw Angle of a Device in a Gravitational Reference System Using Measurements of Motion Sensors and a Magnetometer Attached to the Device |
US20150177020A1 (en) * | 2012-08-02 | 2015-06-25 | Memsic, Inc. | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer |
CN103630137A (zh) * | 2013-12-02 | 2014-03-12 | 东南大学 | 一种用于导航系统的姿态及航向角的校正方法 |
CN105180968A (zh) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | 一种imu/磁强计安装失准角在线滤波标定方法 |
CN106643737A (zh) * | 2017-02-07 | 2017-05-10 | 大连大学 | 风力干扰环境下四旋翼飞行器姿态解算方法 |
CN107389092A (zh) * | 2017-06-27 | 2017-11-24 | 上海交通大学 | 一种基于磁传感器辅助的陀螺标定方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
CN110174121A (zh) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | 一种基于地磁场自适应修正的航姿系统姿态解算方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
CN111551175A (zh) * | 2020-05-27 | 2020-08-18 | 北京计算机技术及应用研究所 | 一种航姿参考系统的互补滤波姿态解算方法 |
CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法 |
Non-Patent Citations (7)
Title |
---|
ZHANG TIEMIN ET AL.: "Attitude measure system based on extended Kalman filter for multi-rotors", COMPUTERS AND ELECTRONICS IN AGRICULTURE, vol. 134 * |
弋英民等: "基于扩展卡尔曼滤波的固定翼无人机姿态解算方法", 小型微型计算机系统, vol. 44, no. 11 * |
张宏欣;周穗华;张伽伟;: "三轴磁强计实时自校正算法", 电子学报, no. 07, 31 July 2017 (2017-07-31) * |
施奇杰;曹大平;: "基于扩展卡尔曼滤波的姿态估计算法", 信息技术, no. 06 * |
李乃星;曹培培;刘圣起;于欢;: "载体机动和磁干扰环境下的航姿算法研究", 制导与引信, no. 03 * |
蔡浩原;赵晟霖;崔松叶;李文宽;刘春秀;: "动态磁场校准的九轴惯性融合", 光学精密工程, no. 09, 30 September 2020 (2020-09-30) * |
郁发新;郑阳明;谢长雄;金佳军;金仲和;: "基于磁强计的皮卫星姿态角测量误差", 吉林大学学报(工学版), no. 06 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117419745A (zh) * | 2023-10-13 | 2024-01-19 | 南京理工大学 | 一种基于循环ekf的惯性辅助地磁在线标定方法及系统 |
CN117289322A (zh) * | 2023-11-24 | 2023-12-26 | 江苏领创星通卫星通信科技有限公司 | 一种基于imu、gps与uwb的高精度定位算法 |
CN118070576A (zh) * | 2024-04-24 | 2024-05-24 | 北京益普希环境咨询顾问有限公司 | 一种地下水污染源溯源辨识方法和系统 |
Also Published As
Publication number | Publication date |
---|---|
CN116817896B (zh) | 2024-04-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN116817896B (zh) | 一种基于扩展卡尔曼滤波的姿态解算方法 | |
CN111678538B (zh) | 一种基于速度匹配的动态水平仪误差补偿方法 | |
Hyyti et al. | A DCM Based Attitude Estimation Algorithm for Low‐Cost MEMS IMUs | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
CN111780786A (zh) | 一种三轴tmr传感器在线校准方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
CN112562077B (zh) | 一种融合pdr和先验地图的行人室内定位方法 | |
JP7025215B2 (ja) | 測位システム及び測位方法 | |
CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
WO2022160391A1 (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 | |
CN103299247A (zh) | 用于磁近场的动态追踪及补偿的设备和方法 | |
CN115900770B (zh) | 一种机载环境下磁传感器的在线校正方法和系统 | |
CN114485641A (zh) | 一种基于惯导卫导方位融合的姿态解算方法及装置 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
CN108592943A (zh) | 一种基于opreq方法的惯性系粗对准计算方法 | |
CN111307114B (zh) | 基于运动参考单元的水面舰船水平姿态测量方法 | |
JP2007163388A (ja) | 方位センサおよび記録媒体 | |
CN111141285B (zh) | 一种航空重力测量装置 | |
CN110940357B (zh) | 一种用于旋转惯导单轴自对准的内杆臂标定方法 | |
CN112539777A (zh) | 一种九轴传感器的误差参数标定方法 | |
CN110030991B (zh) | 融合陀螺和磁强计的飞行物高速旋转角运动测量方法 | |
CN114459478B (zh) | 一种基于姿态运动学模型的惯性测量单元数据融合方法 | |
CN113447018B (zh) | 一种水下惯性导航系统的姿态实时估计方法 | |
CN115878939A (zh) | 基于飞行器舵面偏转的高精度动态测量方法 | |
CN110733671B (zh) | 一种小天体自旋角速度动力学修正方法 |
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 |