[发明专利]一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

说明书

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。本发明包括：设定初始参数值；采集载体运动过程中陀螺及磁强计的输出数据作为量测量；测量地理系下地磁梯度张量；测量载体系下地磁梯度张量；进行状态更新；估计k时刻的状态；估计k时刻的状态。本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍，而且通过三轴磁通门磁强计测量获取量测值y_k的方法具有价格低廉的潜在优势。

技术领域

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。

背景技术

姿态测量是现代导航、制导和控制等诸多领域的一个重要问题。目前，载体姿态确定的方法主要可以分为两大类，矢量确定法和状态估计法。矢量确定法是利用几何关系直接计算，当参考坐标系中的两个矢量准确已知的情况下，采用矢量匹配方法获得单次观测的最优解。但很小的水平基准误差就会带来很大的航向误差，无法利用系统动态信息和历史观测信息，不能通过滤波算法提高姿态估计的精度。另一方面，两个矢量准确已知也是很难做到的。这种方法在很大程度上受到观测矢量的精度限制，无法克服观测矢量的不确定性。状态估计法是通过建立动力学或者运动学模型，得到被估计量动态变化模型和测量模型，采用递推的方法计算估计姿态参数和误差参数。它是一种基于统计特性的方法，能够有效克服传感器误差项引起的参考矢量不确定性，得到统计意义上状态量的最优估计值，提高姿态确定的精度。此外该方法可结合载体动态信息和历史观测信息实现递推估计，在提高测量精度的同时还可同步估计姿态速率信息。常见的状态估计法有扩展卡尔曼滤波(EKF)和非线性卡尔曼滤波(UKF)、递推四元数估计(REQUEST)、扩展四元数估计(EQE)等，还有最小模型误差、自适应滤波、预测滤波和鲁棒滤波等用于估计姿态参数。EKF方法是基于对非线性方程的线性化，当系统具有强非线性时，线性化可能引起大的误差甚至造成滤波器的不稳定；UKF可以克服EKF的缺点，但对随机变量非线性变换后概率分布的逼近精度相对较低；REQUEST是一种类EKF方法，它在每一步中应用QUEST算法，可以得到比传统的EKF更高的精度，但却难于扩展估计其它参量，EQE结合了REQUEST和EKF的优点，但它同样不能避免线性化带来的问题。

相比于磁场矢量测量，磁场梯度张量测量具有磁矢量测量的优势，但没有磁矢量测量对地磁场方向敏感的缺点，相比于磁异常测量，磁场梯度张量测量能够测量更多关于实测地点的信息，容易实现对测量数据的三维反演；磁场梯度张量作为观测量，根据载体姿态估计的过程方程和观测方程具有强非线性的特性，提出的一种新的四元数卡尔曼滤波(Cubature Kalman Filter：CKF)算法，算法本身内蕴了对四元数单位长度的限制，不再需要对四元数估计值进行归一化处理；在高维系统中，CKF(Cubature Kalman Filter)的估计精度优于UKF。

本发明提出的一种基于磁场梯度张量测量的新的姿态测量优化递推估计算法思路新，未见文献报道。

发明内容

本发明的目的在于提供一种能够提高估计精度的基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。

本发明的目的是这样实现的：

(1)设定初始参数值：

由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P₀；

(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量；

(3)测量地理系下地磁梯度张量Gⁿ；

根据惯性/地磁组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gⁿ的5个独立分量和

(4)测量载体系下地磁梯度张量G^b；