CN104567871A - 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 - Google Patents
一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 Download PDFInfo
- Publication number
- CN104567871A CN104567871A CN201510016501.3A CN201510016501A CN104567871A CN 104567871 A CN104567871 A CN 104567871A CN 201510016501 A CN201510016501 A CN 201510016501A CN 104567871 A CN104567871 A CN 104567871A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msub
- msubsup
- mtr
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 40
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 45
- 239000013598 vector Substances 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000012937 correction Methods 0.000 claims description 6
- 230000001419 dependent effect Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000001902 propagating effect Effects 0.000 claims description 4
- 238000005295 random walk Methods 0.000 claims description 4
- 239000000654 additive Substances 0.000 claims description 3
- 230000000996 additive effect Effects 0.000 claims description 3
- 150000001875 compounds Chemical class 0.000 claims description 3
- 238000011426 transformation method Methods 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 2
- 238000009987 spinning Methods 0.000 abstract 1
- 238000004364 calculation method Methods 0.000 description 5
- 238000010606 normalization Methods 0.000 description 5
- 238000004088 simulation Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000000844 transformation 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
- 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/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- 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
本发明属于水下地磁辅助导航领域,具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。本发明包括:设定初始参数值;采集载体运动过程中陀螺及磁强计的输出数据作为量测量;测量地理系下地磁梯度张量;测量载体系下地磁梯度张量;进行状态更新;估计k时刻的状态;估计k时刻的状态。本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍,而且通过三轴磁通门磁强计测量获取量测值yk的方法具有价格低廉的潜在优势。
Description
技术领域
本发明属于水下地磁辅助导航领域,具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。
背景技术
姿态测量是现代导航、制导和控制等诸多领域的一个重要问题。目前,载体姿态确定的方法主要可以分为两大类,矢量确定法和状态估计法。矢量确定法是利用几何关系直接计算,当参考坐标系中的两个矢量准确已知的情况下,采用矢量匹配方法获得单次观测的最优解。但很小的水平基准误差就会带来很大的航向误差,无法利用系统动态信息和历史观测信息,不能通过滤波算法提高姿态估计的精度。另一方面,两个矢量准确已知也是很难做到的。这种方法在很大程度上受到观测矢量的精度限制,无法克服观测矢量的不确定性。状态估计法是通过建立动力学或者运动学模型,得到被估计量动态变化模型和测量模型,采用递推的方法计算估计姿态参数和误差参数。它是一种基于统计特性的方法,能够有效克服传感器误差项引起的参考矢量不确定性,得到统计意义上状态量的最优估计值,提高姿态确定的精度。此外该方法可结合载体动态信息和历史观测信息实现递推估计,在提高测量精度的同时还可同步估计姿态速率信息。常见的状态估计法有扩展卡尔曼滤波(EKF)和非线性卡尔曼滤波(UKF)、递推四元数估计(REQUEST)、扩展四元数估计(EQE)等,还有最小模型误差、自适应滤波、预测滤波和鲁棒滤波等用于估计姿态参数。EKF方法是基于对非线性方程的线性化,当系统具有强非线性时,线性化可能引起大的误差甚至造成滤波器的不稳定;UKF可以克服EKF的缺点,但对随机变量非线性变换后概率分布的逼近精度相对较低;REQUEST是一种类EKF方法,它在每一步中应用QUEST算法,可以得到比传统的EKF更高的精度,但却难于扩展估计其它参量,EQE结合了REQUEST和EKF的优点,但它同样不能避免线性化带来的问题。
相比于磁场矢量测量,磁场梯度张量测量具有磁矢量测量的优势,但没有磁矢量测量对地磁场方向敏感的缺点,相比于磁异常测量,磁场梯度张量测量能够测量更多关于实测地点的信息,容易实现对测量数据的三维反演;磁场梯度张量作为观测量,根据载体姿态估计的过程方程和观测方程具有强非线性的特性,提出的一种新的四元数卡尔曼滤波(CubatureKalman Filter:CKF)算法,算法本身内蕴了对四元数单位长度的限制,不再需要对四元数估计值进行归一化处理;在高维系统中,CKF(Cubature Kalman Filter)的估计精度优于UKF。
本发明提出的一种基于磁场梯度张量测量的新的姿态测量优化递推估计算法思路新,未见文献报道。
发明内容
本发明的目的在于提供一种能够提高估计精度的基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。
本发明的目的是这样实现的:
(1)设定初始参数值:
由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P0;
(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量;
(3)测量地理系下地磁梯度张量Gn;
根据惯性/地磁组合导航系统的指示位置,从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gn的5个独立分量和
(4)测量载体系下地磁梯度张量Gb;
根据步骤(2)中磁强计的输出数据测量载体系下磁场分量h1~h10,计算载体系下地磁梯度张量Gb的5个分量;
Lx和Ly分别为xb和yb方向上梯度测量基线长;
(5)k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新:
过程噪声和观测噪声都是加性的,状态空间形式的离散非线性系统为:
xk=f(xk-1)+wk-1
yk=h(xk)+vk
xk∈Rn和yk∈Rm分别为状态向量和量测向量;f(·)和h(·)分别为系统非线性四元数状态方程量测方程;
姿态估计系统的非线性四元数状态方程:
单位四元数:
载体姿态的单位四元数为 载体坐标系中的角速度矢量为
其中, 表示四元数乘运算;
将陀螺测量坐标系与载体质心本体坐标系重合,陀螺角速度输出采用经典模型:
为陀螺输出;β(t)为陀螺漂移;ηv(t)和ηu(t)分别为随机游走和漂移斜坡噪声;选取单位四元数q(t)和陀螺漂移β(t)作为系统的状态向量,即X=[q(t)T β(t)T]T代表系统的状态向量;
姿态估计系统的非线性四元数量测方程;选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量,建立系统的观测方程为:
Gn和Gb分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示,和是Gb的5个独立分量,和是Gn的5个独立分量,Tij=Tij(q0,q1,q2,q3),i=1,2,3;j=1,2,3为矩阵的元素,且有
h(X)是与状态有关的非线性函数,观测噪声向量v为协方差为R的零均值白噪声;
(5.1)k-1时刻利用四元数对数反对数切换的时间更新;
利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差,采用对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差,算法采用对数变换的Cubature卡尔曼滤波;
(5.1.1)初始化:q=q0;
(5.1.2)主循环:对于i=1,2,…2n,计算xi=logq(qi),令其中wi为CKF中的加权系数;
(5.1.3)如果足够小或超过最大迭代次数,终止循环,输出q,否则,继续循环;
(5.1.4)利用最后一次循环的xi计算协方差矩阵
(5.2)k-1时刻利用CKF的时间更新;
利用标准CKF算法估计Xi,k|k-1中除四元数以外参量的均值,即陀螺漂移部分的均值;
(5.3)将步骤(5.1)利用LTCKF算法得到的四元数部分均值和步骤(5.2)利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值Pk|k-1为:
其中wk为系统噪声,Qk-1为系统噪声协方差;
(6)利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差Pk|k;
根据步骤5.3得到的误差协方差Pk|k-1按照CKF标准算法确定Cubature点集Xi,k|k-1;
将得到的点集Xi,k|k-1通过与状态有关的非线性函数h(Xi,k|k-1)传播Cubature点得到点集Yi,k|k-1:
Yi,k|k-1=h(Xi,k|k-1)
计算k时刻的量测预测值:
计算自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1:
Rk是量测噪声协方差,再根据自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1计算卡尔曼增益Kk:
利用k时刻通过步骤(2)的得到的新的量测值yk对该时刻预测值进行校正,求出状态估计和状态协方差矩阵Pk|k:
k=k+1,转至步骤(5);
(7)对姿态及陀螺漂移进行校正:
利用状态估计值,确定四元数估计值以及陀螺漂移估计值,获得修正后k时刻姿态及陀螺漂移;
四元数估计值用和表示,对k时刻姿态角进行修正:
其中式中,为载体偏航角估计值,θ为俯仰角估计值,为横滚角估计值,
(8)姿态估计系统的运行时间为N,若k=N输出姿态及陀螺漂移结果,若k<N,重复步骤(5)-(7),直到姿态估计系统运行结束。
本发明的有益效果在于:本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍,而且通过三轴磁通门磁强计测量获取量测值yk的方法具有价格低廉的潜在优势。
附图说明
图1是算法流程图;
图2是地磁梯度张量测量配置图;
图3是偏航角误差滤波算法对应的欧拉角误差曲线;
图4是俯仰角误差滤波算法对应的欧拉角误差曲线;
图5是横滚角误差滤波算法对应的欧拉角误差曲线。
具体实施方式
下面结合附图对本发明做进一步描述:
图1给出了本发明的具体流程图。下面结合附图对本发明的具体实施方式进行详细描述:
设过程噪声和观测噪声都是加性的,状态空间形式的离散非线性系统为
xk=f(xk-1)+wk-1 (1)
yk=h(xk)+vk (2)
式中,xk∈Rn和yk∈Rm分别为状态向量和量测向量;f(·)和h(·)为系统非线性状态方程和量测方程;过程噪声wk和量测噪声vk为互不相关的高斯白噪声,且均值和协方差矩阵分别为:
其中δkj为Kronecker-delta函数,Q是非负定对称矩阵,R是正定对称矩阵。
载体姿态估计的状态方程和量测方程具有强非线性的特性,线性化可能引起大的误差甚至造成滤波器的不稳定。提出了一种新的基于地磁梯度张量的四元数Cubature卡尔曼滤波算法,其具体流程如下:
步骤1、设定初始参数值
由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P0。
步骤2、采集载体运动过程中陀螺及磁强计的输出数据作为量测量
步骤3、建立姿态估计的非线性四元数模型
步骤3.1建立姿态估计系统的非线性四元数状态方程单位四元数
设表示载体姿态的单位四元数为 载体坐标系中的角速度矢量为 则有
其中, 表示四元数乘运算。
为简单起见,假设陀螺测量坐标系与载体质心本体坐标系重合,陀螺角速度输出采用经典模型:
式中,为陀螺输出;β(t)为陀螺漂移;ηv(t)和ηu(t)分别为随机游走和漂移斜坡噪声。
选取单位四元数q(t)和陀螺漂移β(t)作为系统的状态向量,即X=[q(t)T β(t)T]T代表系统的状态向量。
由方程(5)和(6)建立载体姿态估计系统的非线性四元数状态方程
步骤3.2建立姿态估计系统的非线性四元数量测方程
选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量,由式建立系统的观测方程为
Gn和Gb分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示,和是Gb的5个独立分量,和是Gn的5个独立分量。Tij=Tij(q0,q1,q2,q3),i=1,2,3;j=1,2,3为矩阵的元素,且有
h(X)是与状态有关的非线性函数,观测噪声向量v为协方差为R的零均值白噪声。
步骤4测量地理系下地磁梯度张量Gn
根据惯性/地磁组合导航系统的指示位置,从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gn的5个独立分量和
步骤5测量载体系下地磁梯度张量Gb
根据步骤2中磁强计(配置方式如图2所示)的输出数据测量载体系下磁场分量h1~h10,按(9)式计算载体系下地磁梯度张量Gb的5个分量;
Lx和Ly分别为xb和yb方向上梯度测量基线长。
步骤6k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新
步骤6.1k-1时刻利用四元数对数反对数切换的时间更新
利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差。
本发明采用如下对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差,算法定义为对数变换的Cubature卡尔曼滤波(LTCKF),具体描述如下:
为叙述方便,对于s,q∈H1,u∈R3,定义如下变换
logq≡αn=u (10)
记
对k-1时刻状态估计中的四元数部分利用(10)定义的对数函数变换为R3中的向量后与其他状态组成新的根据乔里斯基因子Sk-1|k-1和按公式(14)计算确定Cubature点集
记(n-1)维单位列向量e=[1,0,0…,0]T,符号[1]表示对e的元素进行全排列和改变元素符号产生的点集,称为完整全对称点集,[1]i表示点集[1]中的第i个点。
将点集中对应于四元数的部分利用公式(11)变换为四元数后构成χk-1,将χk-1与中与陀螺漂移β(t)对应的部分构成新的Xi,k-1|k-1,然后通过系统的状态方程f(Xi,k-1|k-1)根据公式(15)传播Cubature点得到点集Xi,k|k-1。
Xi,k|k-1=f(Xi,k-1|k-1) (15)
对于Xi,k|k-1中的四元数部分,利用公式(10)~(13)计算其加权均值,即均值q和协方差矩阵Pq:
具体算法如下:
①初始化:q=q0;
②主循环:对于i=1,2,…2n,计算xi=logq(qi),令其中wi为CKF中的加权系数;
③如果足够小或超过最大迭代次数,终止循环,输出q,否则,继续循环。
④利用最后一次循环的xi计算协方差矩阵
步骤6.2k-1时刻利用CKF的时间更新
利用标准CKF算法估计Xi,k|k-1中除四元数以外参量的均值,即陀螺漂移部分的均值。
步骤6.3将步骤6.1利用LTCKF算法得到的四元数部分均值和步骤6.2利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值Pk|k-1按下式计算:
其中wk为系统噪声,Qk-1为系统噪声协方差。
完成时间更新。
步骤7利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差Pk|k
根据步骤6.3得到的误差协方差Pk|k-1按照CKF标准算法确定Cubature点集Xi,k|k-1。
将得到的点集Xi,k|k-1通过步骤3.2获得的与状态有关的非线性函数h(Xi,k|k-1)传播Cubature点得到点集Yi,k|k-1。
Yi,k|k-1=h(Xi,k|k-1) (19)
将Yi,k|k-1代入公式(20)计算k时刻的量测预测值
将以上得到的结果分别代入公式(21)和(22)计算自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1
Rk是量测噪声协方差。再根据自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1计算卡尔曼增益Kk
利用k时刻通过步骤2的得到的新的量测值yk对该时刻预测值进行校正,即根据公式(24)和(25)求出状态估计和状态协方差矩阵Pk|k。
k=k+1,转至步骤6。
步骤8对姿态及陀螺漂移进行校正
利用状态估计值,确定四元数估计值及陀螺漂移估计值,获得修正后k时刻姿态及陀螺漂移。
四元数估计值用和表示,根据下式对k时刻姿态角进行修正。
其中式中,为载体偏航角估计值,θ为俯仰角估计值,为横滚角估计值。
步骤9姿态估计系统的运行时间为N,若k=N输出姿态及陀螺漂移结果,若k<N,重复步骤6-8,直到姿态估计系统运行结束。
分别采用扩展卡尔曼滤波(EKF)、CKF和LTCKF算法同步处理完全相同的仿真数据,初始状态值及其协方差也完全相同,滤波估计结果如图3、4、5所示,图3、4和5分别表示的是每一步估计值对应的偏航角、俯仰角和横滚角误差。从图3、4、5可以看出,对数变换的Cubature卡尔曼滤波算法对姿态角的估计精度比Cubature卡尔曼滤波算法高出数倍,而Cubature卡尔曼滤波算法对姿态角的估计精度比EKF算法也高出数倍。这是因为姿态估计系统的状态方程及观测方程均具有强的非线性,EKF算法存在较大的线性化误差,而Cubature卡尔曼滤波算法是一种非线性滤波,避免了线性化带来的误差及滤波器的不稳定。另一方面,Cubature卡尔曼滤波算法存在四元数加权均值规范化问题及四元数协方差计算奇异性问题,而对数变换的Cubature卡尔曼滤波算法(LTCKF)采用了四元数对数与指数函数进行四元数的均值和方差计算,因此在理论上严格保证了四元数估计值具有单位长度,避免了四元数非归一化带来的计算误差。
本发明的有益效果通过下面仿真实验进行说明
采用欧拉角设定载体的角运动分别为:
式中,ψ为偏航角,θ为俯仰角,γ为横滚角。
仍用一个磁偶极子模型模拟地磁异常,磁偶极子磁矩分量分别为mx=109A·m2,my=2×108A·m2和mz=108A·m2,地磁梯度测量装置相对于磁偶极子的位置分量分别为x=100m,y=50m和z=20m,磁场梯度测量基线长Δx=Δy=1m,,磁场梯度张量测量精度设为5nT/m。仿真时间为180s,采样频率为100Hz。随机游走方差σv=0.03°/s,漂移斜坡噪声方差σu=0.003°/s。分别采用扩展卡尔曼滤波(EKF)、CKF和LTCKF算法同步处理完全相同的仿真数据,初始状态值及其协方差也完全相同,为了便于进一步比较,将估计误差的标准差进行了统计运算,如表1所示。
表1 欧拉角估计误差标准差
从表1可以看出,对数变换的Cubature卡尔曼滤波算法对姿态角的估计精度比Cubature卡尔曼滤波算法高出数倍,而Cubature卡尔曼滤波算法对姿态角的估计精度比EKF算法也高出数倍。这是因为姿态估计系统的状态方程及观测方程均具有强的非线性,EKF算法存在较大的线性化误差,而Cubature卡尔曼滤波算法是一种非线性滤波,避免了线性化带来的误差及滤波器的不稳定。另一方面,Cubature卡尔曼滤波算法存在四元数加权均值规范化问题及四元数协方差计算奇异性问题,而对数变换的Cubature卡尔曼滤波算法(LTCKF)采用了四元数对数与指数函数进行四元数的均值和方差计算,因此在理论上严格保证了四元数估计值具有单位长度,避免了四元数非归一化带来的计算误差。
Claims (1)
1.一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法,其特征在于,包括如下步骤:
(1)设定初始参数值:
由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P0;
(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量;
(3)测量地理系下地磁梯度张量Gn;
根据惯性/地磁组合导航系统的指示位置,从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gn的5个独立分量和
(4)测量载体系下地磁梯度张量Gb;
根据步骤(2)中磁强计的输出数据测量载体系下磁场分量h1~h10,计算载体系下地磁梯度张量Gb的5个分量;
Lx和Ly分别为xb和yb方向上梯度测量基线长;
(5)k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新:
过程噪声和观测噪声都是加性的,状态空间形式的离散非线性系统为:
xk=f(xk-1)+wk-1
yk=h(xk)+vk
xk∈Rn和yk∈Rm分别为状态向量和量测向量;f(·)和h(·)分别为系统非线性四元数状态方程量测方程;
姿态估计系统的非线性四元数状态方程:
单位四元数:
载体姿态的单位四元数为 载体坐标系中的角速度矢量为
其中, 表示四元数乘运算;
将陀螺测量坐标系与载体质心本体坐标系重合,陀螺角速度输出采用经典模型:
为陀螺输出;β(t)为陀螺漂移;ηv(t)和ηu(t)分别为随机游走和漂移斜坡噪声;选取单位四元数q(t)和陀螺漂移β(t)作为系统的状态向量,即X=[q(t)T β(t)T]T代表系统的状态向量;
姿态估计系统的非线性四元数量测方程;选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量,建立系统的观测方程为
Gn和Gb分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示,和是Gb的5个独立分量,和是Gn的5个独立分量,Tij=Tij(q0,q1,q2,q3),i=1,2,3;j=1,2,3为矩阵的元素,且有
h(X)是与状态有关的非线性函数,观测噪声向量v为协方差为R的零均值白噪声;
(5.1)k-1时刻利用四元数对数反对数切换的时间更新;
利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差,采用对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差,算法采用对数变换的Cubature卡尔曼滤波;
(5.1.1)初始化:q=q0;
(5.1.2)主循环:对于i=1,2,…2n,计算xi=logq(qi),令其中wi为CKF中的加权系数;
(5.1.3)如果足够小或超过最大迭代次数,终止循环,输出q,否则,继续循环;
(5.1.4)利用最后一次循环的xi计算协方差矩阵
(5.2)k-1时刻利用CKF的时间更新;
利用标准CKF算法估计Xi,k|k-1中除四元数以外参量的均值,即陀螺漂移部分的均值;
(5.3)将步骤(5.1)利用LTCKF算法得到的四元数部分均值和步骤(5.2)利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值Pk|k-1为:
其中wk为系统噪声,Qk-1为系统噪声协方差;
(6)利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差Pk|k;
根据步骤5.3得到的误差协方差Pk|k-1按照CKF标准算法确定Cubature点集Xi,k|k-1;
将得到的点集Xi,k|k-1通过与状态有关的非线性函数h(Xi,k|k-1)传播Cubature点得到点集Yi,k|k-1:
Yi,k|k-1=h(Xi,k|k-1)
计算k时刻的量测预测值:
计算自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1:
Rk是量测噪声协方差,再根据自相关协方差矩阵Pyy,k|k-1和互相关协方差矩阵Pxy,k|k-1计算卡尔曼增益Kk:
利用k时刻通过步骤(2)的得到的新的量测值yk对该时刻预测值进行校正,求出状态估计和状态协方差矩阵Pk|k:
k=k+1,转至步骤(5);
(7)对姿态及陀螺漂移进行校正:
利用状态估计值,确定四元数估计值以及陀螺漂移估计值,获得修正后k时刻姿态及陀螺漂移;
四元数估计值用和表示,对k时刻姿态角进行修正:
其中式中,为载体偏航角估计值,θ为俯仰角估计值,为横滚角估计值,
(8)姿态估计系统的运行时间为N,若k=N输出姿态及陀螺漂移结果,若k<N,重复步骤(5)-(7),直到姿态估计系统运行结束。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510016501.3A CN104567871B (zh) | 2015-01-12 | 2015-01-12 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510016501.3A CN104567871B (zh) | 2015-01-12 | 2015-01-12 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104567871A true CN104567871A (zh) | 2015-04-29 |
CN104567871B CN104567871B (zh) | 2018-07-24 |
Family
ID=53084487
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510016501.3A Active CN104567871B (zh) | 2015-01-12 | 2015-01-12 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104567871B (zh) |
Cited By (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104898681A (zh) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN105300379A (zh) * | 2015-10-13 | 2016-02-03 | 上海新纪元机器人有限公司 | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 |
CN105606096A (zh) * | 2016-01-28 | 2016-05-25 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105652325A (zh) * | 2016-01-05 | 2016-06-08 | 吉林大学 | 基于指数拟合-自适应卡尔曼的地空电磁数据去噪方法 |
CN106125026A (zh) * | 2016-06-12 | 2016-11-16 | 哈尔滨工程大学 | 一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法 |
CN106595669A (zh) * | 2016-12-27 | 2017-04-26 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN106767776A (zh) * | 2016-11-17 | 2017-05-31 | 上海兆芯集成电路有限公司 | 移动设备及求取移动设备姿态的方法 |
CN108089434A (zh) * | 2017-12-11 | 2018-05-29 | 北京控制工程研究所 | 一种基于磁强计的皮纳卫星姿态捕获方法 |
CN108844536A (zh) * | 2018-04-04 | 2018-11-20 | 中国科学院国家空间科学中心 | 一种基于量测噪声协方差矩阵估计的地磁导航方法 |
CN109000639A (zh) * | 2018-06-05 | 2018-12-14 | 哈尔滨工程大学 | 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 |
CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
CN109633490A (zh) * | 2019-01-23 | 2019-04-16 | 中国科学院上海微系统与信息技术研究所 | 一种全张量磁梯度测量组件标定系统及标定方法 |
CN110440796A (zh) * | 2019-08-19 | 2019-11-12 | 哈尔滨工业大学 | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 |
CN110470294A (zh) * | 2019-08-09 | 2019-11-19 | 西安电子科技大学 | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 |
CN110672078A (zh) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | 一种基于地磁信息的高旋弹丸姿态估计方法 |
CN112504266A (zh) * | 2020-11-17 | 2021-03-16 | 哈尔滨工程大学 | 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 |
CN112611310A (zh) * | 2020-12-11 | 2021-04-06 | 哈尔滨工程大学 | 一种磁偶极子目标测距测向方法 |
CN113340297A (zh) * | 2021-04-22 | 2021-09-03 | 中国人民解放军海军工程大学 | 基于坐标系传递的姿态估计方法、系统、终端、介质及应用 |
CN114609555A (zh) * | 2020-12-08 | 2022-06-10 | 北京自动化控制设备研究所 | 集群无人磁总场全轴梯度探测方法及使用其的探测系统 |
CN117419745A (zh) * | 2023-10-13 | 2024-01-19 | 南京理工大学 | 一种基于循环ekf的惯性辅助地磁在线标定方法及系统 |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101290229A (zh) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | 硅微航姿系统惯性/地磁组合方法 |
CN101782391A (zh) * | 2009-06-22 | 2010-07-21 | 北京航空航天大学 | 机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法 |
CN101915579A (zh) * | 2010-07-15 | 2010-12-15 | 哈尔滨工程大学 | 一种基于ckf的sins大失准角初始对准新方法 |
KR20120062519A (ko) * | 2010-12-06 | 2012-06-14 | 국방과학연구소 | 관성 항법 시스템의 바이어스 추정치를 이용한 정렬 장치 및 그 항법 시스템 |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 |
CN103455675A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种基于ckf的非线性异步多传感器信息融合方法 |
CN103454662A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种基于ckf的sins/北斗/dvl组合对准方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103557864A (zh) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Mems捷联惯导自适应sckf滤波的初始对准方法 |
WO2014022664A2 (en) * | 2012-08-02 | 2014-02-06 | Memsic, Inc. | Method and apparatus for data fusion of a three axis magnetometer and three axis accelerometer |
CN103604430A (zh) * | 2013-11-06 | 2014-02-26 | 哈尔滨工程大学 | 一种基于边缘化ckf重力辅助导航的方法 |
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
CN103900608A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
-
2015
- 2015-01-12 CN CN201510016501.3A patent/CN104567871B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101290229A (zh) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | 硅微航姿系统惯性/地磁组合方法 |
CN101782391A (zh) * | 2009-06-22 | 2010-07-21 | 北京航空航天大学 | 机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法 |
CN101915579A (zh) * | 2010-07-15 | 2010-12-15 | 哈尔滨工程大学 | 一种基于ckf的sins大失准角初始对准新方法 |
KR20120062519A (ko) * | 2010-12-06 | 2012-06-14 | 국방과학연구소 | 관성 항법 시스템의 바이어스 추정치를 이용한 정렬 장치 및 그 항법 시스템 |
WO2014022664A2 (en) * | 2012-08-02 | 2014-02-06 | Memsic, Inc. | Method and apparatus for data fusion of a three axis magnetometer and three axis accelerometer |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 |
CN103455675A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种基于ckf的非线性异步多传感器信息融合方法 |
CN103454662A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种基于ckf的sins/北斗/dvl组合对准方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103557864A (zh) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Mems捷联惯导自适应sckf滤波的初始对准方法 |
CN103604430A (zh) * | 2013-11-06 | 2014-02-26 | 哈尔滨工程大学 | 一种基于边缘化ckf重力辅助导航的方法 |
CN103900608A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
CN103900574A (zh) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于迭代容积卡尔曼滤波姿态估计方法 |
Non-Patent Citations (6)
Title |
---|
傅建国等: "一种基于地球重力场和磁场的姿态估计新算法", 《电子学报》 * |
张晓霞等: "基于磁强计/陀螺的卡尔曼滤波定姿算法", 《探测与控制学报》 * |
贾瑞才: "光学精密工程", 《光学精密工程》 * |
贾瑞才: "基于四元数EKF的低成本MEMS姿态估计算法", 《传感技术学报》 * |
郭庆等: "宇航学报", 《宇航学报》 * |
黄玉等: "载体姿态变化对水下磁场定位精度的影响及监测方法", 《系统工程与电子技术》 * |
Cited By (33)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104898681B (zh) * | 2015-05-04 | 2017-07-28 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN104898681A (zh) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN105300379A (zh) * | 2015-10-13 | 2016-02-03 | 上海新纪元机器人有限公司 | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 |
CN105300379B (zh) * | 2015-10-13 | 2017-12-12 | 上海新纪元机器人有限公司 | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 |
CN105652325A (zh) * | 2016-01-05 | 2016-06-08 | 吉林大学 | 基于指数拟合-自适应卡尔曼的地空电磁数据去噪方法 |
CN105652325B (zh) * | 2016-01-05 | 2017-09-19 | 吉林大学 | 基于指数拟合‑自适应卡尔曼的地空电磁数据去噪方法 |
CN105606096B (zh) * | 2016-01-28 | 2018-03-30 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105606096A (zh) * | 2016-01-28 | 2016-05-25 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN106125026A (zh) * | 2016-06-12 | 2016-11-16 | 哈尔滨工程大学 | 一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法 |
CN106125026B (zh) * | 2016-06-12 | 2019-02-26 | 哈尔滨工程大学 | 一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法 |
CN106767776A (zh) * | 2016-11-17 | 2017-05-31 | 上海兆芯集成电路有限公司 | 移动设备及求取移动设备姿态的方法 |
CN106595669A (zh) * | 2016-12-27 | 2017-04-26 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN106595669B (zh) * | 2016-12-27 | 2023-04-11 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN108089434B (zh) * | 2017-12-11 | 2021-06-11 | 北京控制工程研究所 | 一种基于磁强计的皮纳卫星姿态捕获方法 |
CN108089434A (zh) * | 2017-12-11 | 2018-05-29 | 北京控制工程研究所 | 一种基于磁强计的皮纳卫星姿态捕获方法 |
CN108844536A (zh) * | 2018-04-04 | 2018-11-20 | 中国科学院国家空间科学中心 | 一种基于量测噪声协方差矩阵估计的地磁导航方法 |
CN108844536B (zh) * | 2018-04-04 | 2020-07-03 | 中国科学院国家空间科学中心 | 一种基于量测噪声协方差矩阵估计的地磁导航方法 |
CN109000639A (zh) * | 2018-06-05 | 2018-12-14 | 哈尔滨工程大学 | 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 |
CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
CN109633490B (zh) * | 2019-01-23 | 2021-04-02 | 中国科学院上海微系统与信息技术研究所 | 一种全张量磁梯度测量组件的标定方法 |
CN109633490A (zh) * | 2019-01-23 | 2019-04-16 | 中国科学院上海微系统与信息技术研究所 | 一种全张量磁梯度测量组件标定系统及标定方法 |
CN110470294A (zh) * | 2019-08-09 | 2019-11-19 | 西安电子科技大学 | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 |
CN110440796A (zh) * | 2019-08-19 | 2019-11-12 | 哈尔滨工业大学 | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 |
CN110672078A (zh) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | 一种基于地磁信息的高旋弹丸姿态估计方法 |
CN110672078B (zh) * | 2019-10-12 | 2021-07-06 | 南京理工大学 | 一种基于地磁信息的高旋弹丸姿态估计方法 |
CN112504266A (zh) * | 2020-11-17 | 2021-03-16 | 哈尔滨工程大学 | 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 |
CN112504266B (zh) * | 2020-11-17 | 2022-06-17 | 哈尔滨工程大学 | 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 |
CN114609555A (zh) * | 2020-12-08 | 2022-06-10 | 北京自动化控制设备研究所 | 集群无人磁总场全轴梯度探测方法及使用其的探测系统 |
CN114609555B (zh) * | 2020-12-08 | 2024-05-03 | 北京自动化控制设备研究所 | 集群无人磁总场全轴梯度探测方法及使用其的探测系统 |
CN112611310A (zh) * | 2020-12-11 | 2021-04-06 | 哈尔滨工程大学 | 一种磁偶极子目标测距测向方法 |
CN112611310B (zh) * | 2020-12-11 | 2022-09-27 | 哈尔滨工程大学 | 一种磁偶极子目标测距测向方法 |
CN113340297A (zh) * | 2021-04-22 | 2021-09-03 | 中国人民解放军海军工程大学 | 基于坐标系传递的姿态估计方法、系统、终端、介质及应用 |
CN117419745A (zh) * | 2023-10-13 | 2024-01-19 | 南京理工大学 | 一种基于循环ekf的惯性辅助地磁在线标定方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN104567871B (zh) | 2018-07-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104567871B (zh) | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 | |
CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
Gao et al. | Maximum likelihood principle and moving horizon estimation based adaptive unscented Kalman filter | |
CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
CN105043388B (zh) | 基于惯性/重力匹配组合导航的向量搜索迭代匹配方法 | |
CN103630137B (zh) | 一种用于导航系统的姿态及航向角的校正方法 | |
CN105737823B (zh) | 一种基于五阶ckf的gps/sins/cns组合导航方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN105300384A (zh) | 一种用于卫星姿态确定的交互式滤波方法 | |
CN110398257A (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
CN107270891B (zh) | 基于抗差估计的惯性地磁匹配定位方法 | |
CN104697523A (zh) | 基于迭代计算的惯性/地磁匹配定位方法 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN103776449B (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
CN106200377A (zh) | 一种飞行器控制参数的估计方法 | |
CN105447574A (zh) | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 | |
CN105066996A (zh) | 自适应矩阵卡尔曼滤波姿态估计方法 | |
CN107389069A (zh) | 基于双向卡尔曼滤波的地面姿态处理方法 | |
CN109000638A (zh) | 一种小视场星敏感器量测延时滤波方法 | |
CN103575298A (zh) | 基于自调节的ukf失准角初始对准方法 | |
CN112446010B (zh) | 自适应弱敏秩卡尔曼滤波方法及其应用 |
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 |