CN111982089A - 一种磁罗盘全罗差实时校准与补偿方法 - Google Patents

Abstract

提供一种磁罗盘全罗差实时校准与补偿方法，属于导航技术领域，分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变，由不同姿态下的磁罗盘三轴磁场数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，实现全罗差补偿。本发明解决现有技术无法实现对磁罗盘中全罗差12个参数进行校正和补偿的问题，不需要附加的姿态测量设备就能够提供准确的外部姿态信息，精度高、成本低及操作简便等特点，实现了全罗差实时校正，扩展了校正与补偿方法的自主性。

Description

一种磁罗盘全罗差实时校准与补偿方法

技术领域

本发明属于导航技术领域，具体涉及一种磁罗盘全罗差实时校准与补偿方法。

背景技术

磁罗盘是一种利用地球磁场与重力加速度实现航向测量的工具，由于其具有体积小、功耗低、可靠性高、精度高等方面的优势，磁罗盘作为导航仪器、姿态传感器或动作捕捉设备已被广泛应用于矿山探测、煤田开采、航空、航天、机器人、航海、导航等领域。在实际应用中，由于大多数载体上均存在类似电缆、电机、钢铁结构等铁磁性物体，磁罗盘极易受到载体周边杂散磁场地干扰，造成较大的磁航向测量误差(罗差)。为实现高精度、高可靠性的导航与定位，必须对磁罗盘罗差进行有效的校正与补偿。目前的做法是通过对误差产生机理分析，建立误差模型，再通过不同方法对误差参数进行估计，例如：椭圆假设法、分步校准法、椭球假设法、幅值约束法、位置翻转法等，再尝试不同方法对模型参数进行离线估计。目前磁罗盘罗差校正主要存在以下局限性：(1)只校正罗差中的9个参数，不能对磁罗盘中全罗差12个参数进行校正；(2)即使能够校正磁罗盘全罗差参数，也必须配备外部辅助测量设备，如测姿设备等，这就增加了校正系统的使用成本，失去了校正与补偿方法的经济性；(3)上述方法都需要对磁罗盘输出进行批处理，无法实现实时校正。因此有必要提出改进。

发明内容

本发明解决的技术问题：提供一种磁罗盘全罗差实时校准与补偿方法，本发明是一种具有精度高、成本低及操作简便等特点的磁罗盘全罗差实时校正与补偿方法，对磁罗盘全罗差实时校正与补偿方法分阶段进行，提升了磁航向精度，解决在没有附加测试设备的情况下不能对磁罗盘中全罗差进行校正与补偿的问题，解决现有技术无法实现对磁罗盘中全罗差12个参数进行校正和补偿的问题，实现了全罗差实时校正，扩展了校正与补偿方法的自主性。

本发明采用的技术方案：一种磁罗盘全罗差实时校准与补偿方法，该方法分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变性，由不同姿态下的磁罗盘三轴磁场测量数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，实现全罗差补偿；

具体包括如下步骤：

步骤1：在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||，采集载体不同姿态下的磁罗盘三轴磁场数据；

步骤2：建立全罗差模型和校正模型

定义

为地磁场矢量的真实值在载体坐标系中的投影，

为磁罗盘三轴磁场的测量值；

地磁场的真实值H^b与磁罗盘三轴磁场的测量值H^m之间可用下列矩阵方程表示：

H^m＝CH^b+b+n

其中，矩阵C＝I+K，

为软磁干扰系数矩阵，b＝[b_x b_y b_z]^T为硬磁干扰系数向量，向量n表示磁传感器噪声，可以用测量值的平均值来估计；相对于系统误差，测量噪声误差的数量级较小，为此，在建立标定与补偿模型时可先不予考虑n的影响；

为了消除上述系统误差，根据全罗差模型构建校正模型，校正模型以磁罗盘中三轴磁场测量值H^m作为模型输入，其模型的输出为

其中

为地磁场的估计值，矩阵G是矩阵C的逆矩阵，若能利用H^m求得矩阵G的估计和向量b的估计，则可根据上式由磁罗盘三轴磁场测量数据获得地磁场矢量的估计值；

步骤3：构造Kalman滤波模型

对于步骤2中的校正模型的输出公式，有

||H^b||²＝[G(H^m-b)]^T[G(H^m-b)]

＝(H^m-b)^TΓ(H^m-b)

其中：矩阵

γ是待估计参数；

展开方程，则

为磁罗盘三轴磁场的测量值；

以待估计参数作为状态变量，基于地磁场模的不变性，构造观测方程，得到如下的Kalman滤波模型：

y＝Hx+v

式中，

v＝2(H^m-b)^TΓn+n^TΓn为测量噪声，其均值μ和方差σ可以表示为

μ＝E(v)＝sum(Γ.*∑)

σ²＝E(v²)-μ²

＝4[Γ(H^m-b)]^T∑[Γ(H^m-b)]

H和x分别为9元素行向量和9元素列向量，其各元素分别为：

h₉＝1

对Kalman滤波模型测量方程式离散化，可以得到：

式中，下标表示k时刻，噪声v_k是一个与状态相关的非高斯噪声，若采用传统的最小二乘算法无法实现参数的最优估计；

步骤4：采用自适应Kalman滤波进行状态估计；

步骤5：由步骤4得到的最优估计值

按照下式计算矩阵K_G和矢量b的最优估计值

和

1)计算矢量

2)计算矩阵Γ

3)由矩阵Γ来确定矩阵

Γ用来确定矩阵K_G，K_G由矩阵G的极分解给出，即

K_G＝(R_G)^TG

所以，由矩阵Γ的奇异值分解，可以得到：

步骤6：根据估计的误差矩阵

和矢量

带入校正模型，得到第一阶段校正后的地磁矢量估计值

即

步骤7：第二阶段确定剩余罗差参数，实现全罗差补偿；

基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失3个罗差参数，从而实现全罗差补偿；

地磁场矢量的模长与方向均不变，因此其竖直分矢量与水平分矢量均为常矢量，计算地磁场竖直分量：

其中F(θ,γ)＝[sinγ -sinθcosγ cosθcosγ]^T为旋转矩阵

的第3行列向量；

根据空间轴系的转换关系，将R_G简化成3参数的方向余弦矩阵，得到正交矩阵R_G如下：

该式表示为一个关于参数β₁、β₂和β₃的非线性方程，估计出该参数方程，即可确定正交矩阵R_G，再根据G＝R_GK_G，确定矩阵G，从而完成实现全罗差补偿；

步骤8：根据估计的误差矩阵

矢量

和正交矩阵

得到最终全罗差补偿后的磁罗盘三轴磁场

即

步骤9：根据补偿后的磁罗盘的三轴磁场数据，结合三轴加速度数据按照以下公式计算得到磁罗盘的俯仰角、横滚角和磁航向角；

计算俯仰角θ和横滚角γ：

其中，

为磁罗盘载体坐标系下测得三轴加速度；

将经过校正和补偿后的载体坐标系下磁罗盘三轴磁场数据

和

变换到地理坐标下得到水平方向两轴磁场

和

其计算公式如下：

利用下式计算磁航向角ψ：

上述步骤4中，Kalman滤波进行状态估计的步骤：

1)在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||；

2)k＝0时，对状态x、误差协方差P₀和量测噪声R分别进行初始化为

P₀和R₀；

3)令k＝k+1时，载体作改变姿态的机动动作，由捷联于载体的磁传感器获得磁罗盘三轴测量值

4)依据步骤3计算系统的观测值y_k、观测矩阵H_k和噪声v_k；

5)按照下式计算自适应Kalman滤波的新息e_k：

6)按照下式更新量测噪声R_k：

7)按照下式计算滤波增益K_k：

8)按照下式计算状态估计

9)按照下式计算估计误差协方差P_k：

10)令k＝k+1，返回步骤3)。

上述步骤7中，使用粒子群方法对正交矩阵R_G中参数进行估计，其步骤如下:

1)按照下式构造目标函数：

ξ＝[β₁ β₂ β₃]^T为待估计参数向量，n为磁罗盘采样点个数，上式表示

是在使n个采样点

的统计值L(ξ)最小时的参数值；

2)初始化粒子群及参数设置；

随机给定微粒初始速度V_ij与初始位置X_ij，其中V_ij～U[V_min,V_max]，β_ij～U[β_min,β_max]i＝1,2,L,m，j＝1,2,3，m为种群大小；初始个体适应度pbest_i＝L(ξ_i)和全局最优适应度gbest＝min[L(ξ₁) L(ξ₂) L L(ξ_m)]；

3)根据以下公式调整微粒速度和位置

式中:t为当前进化代数，r₁、r₂为均匀分布于(0,1)区间的随机数，ω为惯性权重，c₁、c₂为加速因子；

4)局部位置和全局位置向量更新：更新个体的pbest_i和全局最优gbest；

5)终止条件判断：若达到最大进化代数或满足精度标准，输出全局最优解；否则继续迭代，跳转至步骤3)。。

本发明与现有技术相比的优点：

1、本方案中的磁罗盘全罗差实时校正与补偿方法分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变，由不同姿态下的磁罗盘三轴磁场数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，从而实现全罗差补偿；提升了磁航向精度，解决现有技术无法实现对磁罗盘中全罗差12个参数进行校正和补偿的问题，不需要附加的姿态测量设备就能够提供准确的外部姿态信息；

2、本方案具有精度高、成本低及操作简便等特点，解决在没有附加测试设备的情况下不能对磁罗盘中全罗差进行校正与补偿的问题，实现了全罗差实时校正，扩展了校正与补偿方法的自主性。

附图说明

图1为本发明的原理框图；

图2为本发明中磁罗盘三轴磁传感器的校正模型；

图3为本发明中地磁场在水平面的投影示意图。

具体实施方式

以下结合附图1-3和具体实施例对本发明进行详细描述。

一种磁罗盘全罗差实时校准与补偿方法：

首先对数字磁罗盘作简单介绍，数字磁罗盘由三轴磁传感器、三轴加速度计以及数据采集系统构成，三轴磁传感器用于测量三轴方向磁场数据，三轴加速度计用于测量三轴方向加速度数据。

该方法分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变性，由不同姿态下的磁罗盘三轴磁场测量数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，实现全罗差补偿。提升了磁航向精度，解决现有技术无法实现对磁罗盘中全罗差12个参数进行校正和补偿的问题。

具体包括如下步骤：如图3所示，

步骤1：在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||，采集载体不同姿态下的磁罗盘三轴磁场数据；

步骤2：建立罗差模型和校正模型。

定义

为地磁场矢量的真实值在载体坐标系中的投影，

为磁罗盘三轴磁场的测量值。地磁传感器受到外界环境干扰产生影响，其敏感的磁场强度为地磁场与载体杂散干扰磁场叠加的总和，地磁场的真实值H^b与磁罗盘三轴磁场的测量值H^m之间可用下列矩阵方程表示：

H^m＝CH^b+b+n

其中，矩阵C＝I+K，

为软磁干扰系数矩阵，b＝[b_x b_y b_z]^T为硬磁干扰系数向量，向量n表示磁传感器噪声，可以用测量值的平均值来估计。相对于系统误差，测量噪声误差的数量级较小，为此，在建立标定与补偿模型时可先不予考虑n的影响。

为了消除上述系统误差，根据全罗差模型构建如图1的校正模型。校正模型以磁罗盘三轴磁场的测量值H^m作为模型输入，其模型的输出为

其中

为地磁场的估计值。若能利用H^m求得矩阵G的估计和向量b的估计，则可根据上式由磁罗盘三轴测量数据获得地磁场矢量的估计值。

步骤3：构造Kalman滤波模型。

对于步骤2中的校正模型的输出公式，有

||H^b||²＝[G(H^m-b)]^T[G(H^m-b)]

＝(H^m-b)^TΓ(H^m-b)

其中：矩阵

展开方程，则

为磁罗盘三轴磁场的测量值；

以待估计参数作为状态变量，基于地磁场模的不变性，构造观测方程，，得Kalman滤波测量方程：

y＝Hx+v

式中，

v＝2(H^m-b)^TΓn+n^TΓn为测量噪声，其均值μ和方差σ可以表示为

μ＝E(v)＝sum(Γ.*∑)

σ²＝E(v²)-μ²

＝4[Γ(H^m-b)]^T∑[Γ(H^m-b)]

H和x分别为9元素行向量和9元素列向量，其各元素分别为：

h₉＝1

对Kalman滤波测量方程式离散化，可以得到：

式中，下标表示k时刻。噪声v_k是一个与状态相关的非高斯噪声，若采用传统的最小二乘算法无法实现参数的最优估计。

步骤4：采用自适应Kalman滤波进行状态估计，步骤如下：

1)在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||；

2)k＝0时，对状态x、误差协方差P₀和量测噪声R分别进行初始化为

P₀和R₀；

3)令k＝k+1时，载体作改变姿态的机动动作，由捷联于载体的磁传感器获得磁罗盘三轴测量值

4)依据步骤3计算系统的观测值y_k、观测矩阵H_k和噪声v_k。

5)按照下式计算自适应Kalman滤波的新息e_k：

6)按照下式更新量测噪声R_k：

7)按照下式计算滤波增益K_k：

8)按照下式计算状态估计

9)按照下式计算估计误差协方差P_k：

10)令k＝k+1，返回步骤3)。

步骤5：由步骤4得到的最优估计值

按照以下步骤计算矩阵K_G和矢量b的最优估计值

和

1)计算矢量

2)计算矩阵Γ

3)由矩阵Γ来确定矩阵

因为对于任何一个满足R_G ^TR_G＝I的正交矩阵R_G，都有(R_GG)^T(R_GG)＝G^TR_G ^TR_GG＝Γ，所以Γ用来确定矩阵K_G，K_G由矩阵G的极分解给出，即

K_G＝(R_G)^TG

所以，由矩阵Γ的奇异值分解，可以得到：

步骤6：根据估计的误差矩阵

和矢量

带入校正模型，得到第一阶段校正后的地磁矢量估计值

即

目前的技术方案中只能对12个罗差参数中的9个进行校正，要对剩余缺失3个参数进行校正需要借助外部附加测试设备。

步骤7：第二阶段确定剩余罗差参数，实现全罗差补偿。

由步骤6中公式式可知：

G＝R_GK_G

由上式可知，要校正剩余3个罗差参数，在于确定正交矩阵R_G。这也是现有的文献中提到的要对剩余缺失3个参数进行校正需要借助外部附加测试设备。从几何意义上说，正交矩阵R_G的存在相当于将第一阶段补偿后地磁场测量轴系相对于较重力加速度测量轴系旋转了一定角度，引入了轴系间的非对准误差。而李智等人实验证明了非对准误差的存在会造成较强的非线性罗差。

固定空间位置，地磁场矢量的模与方向均不变，因此其竖直分矢量与水平分矢量均为常矢量，如图2所示。计算地磁场竖直分量：

其中F(θ,γ)＝[sinγ -sinθcosγ cosθcosγ]^T为载体坐标系与地理坐标系之间的转换矩阵

的第3行列向量。

根据空间轴系的转换关系，将R_G简化3参数的方向余弦矩阵，得到正交矩阵R_G如下：

该式表示为一个关于参数β₁、β₂和β₃的非线性方程，估计出该参数方程，即可确定正交矩阵R_G，再根据G＝R_GK_G，确定矩阵G，从而完成实现全罗差补偿；

使用粒子群方法对正交矩阵R_G中参数进行估计，其步骤如下:

1)按照下式构造目标函数：

ξ＝[β₁ β₂ β₃]^T为待估计参数向量，n为磁罗盘采样点个数，上式表示

是在使n个采样点

的统计值L(ξ)最小时的参数值；

2)初始化粒子群及参数设置；

随机给定微粒初始速度V_ij与初始位置X_ij，其中V_ij～U[V_min,V_max]，β_ij～U[β_min,β_max]i＝1,2,L,m，j＝1,2,3，m为种群大小；初始个体适应度pbest_i＝L(ξ_i)和全局最优适应度gbest＝min[L(ξ₁) L(ξ₂) L L(ξ_m)]；

3)根据以下公式调整微粒速度和位置。

式中:t为当前进化代数，r₁、r₂为均匀分布于(0,1)区间的随机数，ω为惯性权重，c₁、c₂为加速因子；

4)局部位置和全局位置向量更新：更新个体的pbest_i和全局最优gbest；

5)终止条件判断：若达到最大进化代数或满足精度标准，输出全局最优解；否则继续迭代，跳转至步骤3)。

步骤8：根据估计的误差矩阵

矢量

和正交矩阵

得到最终全罗差补偿后的磁罗盘三轴磁场

即

步骤9：根据步骤8最终全罗差补偿后的三轴磁场数据，结合三轴加速度数据求解得到磁罗盘的俯仰角、横滚角和磁航向角。

载体坐标系与地理坐标系之间的转换矩阵为：

其中θ、γ和ψ分别为数字磁罗盘的俯仰、横滚和磁航向角。

其中俯仰角和横滚角的求解方法为：利用地理坐标系与载体坐标系之间的变换关系，则有：

其中，

为磁罗盘载体坐标系下测得三轴加速度。

通过对上式进行求解，可计算出俯仰角θ和横滚角γ

将经过校正后的载体坐标系下地磁传感器输出值

和

变换到地理坐标下得到水平方向两轴磁场

和

其计算公式如下：

利用下式计算磁航向角ψ：

本发明是一种具有精度高、成本低及操作简便等特点的磁罗盘全罗差实时校正与补偿方法，对磁罗盘全罗差实时校正与补偿方法分阶段进行，提升了磁航向精度，解决在没有附加测试设备的情况下不能对磁罗盘中全罗差进行校正与补偿的问题，解决现有技术无法实现对磁罗盘中全罗差12个参数进行校正和补偿的问题，实现了全罗差实时校正，扩展了校正与补偿方法的自主性。

上述实施例，只是本发明的较佳实施例，并非用来限制本发明实施范围，故凡以本发明权利要求所述内容所做的等效变化，均应包括在本发明权利要求范围之内。

Claims (3)

1.一种磁罗盘全罗差实时校准与补偿方法，其特征在于：该方法分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变性，由不同姿态下的磁罗盘三轴磁场测量数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，实现全罗差补偿；

具体包括如下步骤：

步骤1：在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||，采集载体不同姿态下的磁罗盘三轴磁场数据；

步骤2：建立全罗差模型和校正模型

定义

为地磁场矢量的真实值在载体坐标系中的投影，

为磁罗盘三轴磁场的测量值；

地磁场的真实值H^b与磁罗盘三轴磁场的测量值H^m之间可用下列矩阵方程表示：

H^m＝CH^b+b+n

其中，矩阵C＝I+K，

为软磁干扰系数矩阵，b＝[b_x b_y b_z]^T为硬磁干扰系数向量，向量n表示磁传感器噪声，可以用测量值的平均值来估计；相对于系统误差，测量噪声误差的数量级较小，为此，在建立标定与补偿模型时可先不予考虑n的影响；

为了消除上述系统误差，根据全罗差模型构建校正模型，校正模型以磁罗盘中三轴磁场测量值H^m作为模型输入，其模型的输出为

其中

为地磁场的估计值，矩阵G是矩阵C的逆矩阵，若能利用H^m求得矩阵G的估计和向量b的估计，则可根据上式由磁罗盘三轴磁场测量数据获得地磁场矢量的估计值；

步骤3：构造Kalman滤波模型

对于步骤2中的校正模型的输出公式，有

||H^b||²＝[G(H^m-b)]^T[G(H^m-b)]

＝(H^m-b)^TΓ(H^m-b)

其中：矩阵

γ是待估计参数；

展开方程，则

为磁罗盘三轴磁场的测量值；

以待估计参数作为状态变量，基于地磁场模的不变性，构造观测方程，得到如下的Kalman滤波模型：

y＝Hx+v

式中，

v＝2(H^m-b)^TΓn+n^TΓn为测量噪声，其均值μ和方差σ可以表示为

μ＝E(v)＝sum(Γ.*∑)

σ²＝E(v²)-μ²

＝4[Γ(H^m-b)]^T∑[Γ(H^m-b)]

H和x分别为9元素行向量和9元素列向量，其各元素分别为：

h₉＝1

对Kalman滤波模型测量方程式离散化，可以得到：

式中，下标表示k时刻，噪声v_k是一个与状态相关的非高斯噪声，若采用传统的最小二乘算法无法实现参数的最优估计；

步骤4：采用自适应Kalman滤波进行状态估计；

步骤5：由步骤4得到的最优估计值

按照下式计算矩阵K_G和矢量b的最优估计值

和

1)计算矢量

2)计算矩阵Γ

3)由矩阵Γ来确定矩阵

Γ用来确定矩阵K_G，K_G由矩阵G的极分解给出，即

K_G＝(R_G)^TG

所以，由矩阵Γ的奇异值分解，可以得到：

步骤6：根据估计的误差矩阵

和矢量

带入校正模型，得到第一阶段校正后的地磁矢量估计值

即

步骤7：第二阶段确定剩余罗差参数，实现全罗差补偿；

基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失3个罗差参数，从而实现全罗差补偿；

地磁场矢量的模长与方向均不变，因此其竖直分矢量与水平分矢量均为常矢量，计算地磁场竖直分量：

其中F(θ,γ)＝[sinγ -sinθcosγ cosθcosγ]^T为旋转矩阵

的第3行列向量；

根据空间轴系的转换关系，将R_G简化成3参数的方向余弦矩阵，得到正交矩阵R_G如下：

该式表示为一个关于参数β₁、β₂和β₃的非线性方程，估计出该参数方程，即可确定正交矩阵R_G，再根据G＝R_GK_G，确定矩阵G，从而完成实现全罗差补偿；

步骤8：根据估计的误差矩阵

矢量

和正交矩阵

得到最终全罗差补偿后的磁罗盘三轴磁场

即

步骤9：根据补偿后的磁罗盘的三轴磁场数据，结合三轴加速度数据按照以下公式计算得到磁罗盘的俯仰角、横滚角和磁航向角；

计算俯仰角θ和横滚角γ：

其中，

为磁罗盘载体坐标系下测得三轴加速度；

将经过校正和补偿后的载体坐标系下磁罗盘三轴磁场数据

和

变换到地理坐标下得到水平方向两轴磁场

和

其计算公式如下：

利用下式计算磁航向角ψ：

2.根据权利要求1所述的一种磁罗盘全罗差实时校准与补偿方法，其特征在于：上述步骤4中，Kalman滤波进行状态估计的步骤：

1)在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||；

2)k＝0时，对状态x、误差协方差P₀和量测噪声R分别进行初始化为

P₀和R₀；

3)令k＝k+1时，载体作改变姿态的机动动作，由捷联于载体的磁传感器获得磁罗盘三轴测量值

4)依据步骤3计算系统的观测值y_k、观测矩阵H_k和噪声v_k；

5)按照下式计算自适应Kalman滤波的新息e_k：

6)按照下式更新量测噪声R_k：

7)按照下式计算滤波增益K_k：

8)按照下式计算状态估计

9)按照下式计算估计误差协方差P_k：

10)令k＝k+1，返回步骤3)。

3.根据权利要求1所述的一种磁罗盘全罗差实时校准与补偿方法，其特征在于：上述步骤7中，使用粒子群方法对正交矩阵R_G中参数进行估计，其步骤如下:

1)按照下式构造目标函数：

ξ＝[β₁ β₂ β₃]^T为待估计参数向量，n为磁罗盘采样点个数，上式表示

是在使n个采样点

的统计值L(ξ)最小时的参数值；

2)初始化粒子群及参数设置；

随机给定微粒初始速度V_ij与初始位置X_ij，其中V_ij～U[V_min,V_max]，β_ij～U[β_min,β_max]i＝1,2,L,m，j＝1,2,3，m为种群大小；初始个体适应度pbest_i＝L(ξ_i)和全局最优适应度gbest＝min[L(ξ₁) L(ξ₂) L L(ξ_m)]；

3)根据以下公式调整微粒速度和位置

式中:t为当前进化代数，r₁、r₂为均匀分布于(0,1)区间的随机数，ω为惯性权重，c₁、c₂为加速因子；

4)局部位置和全局位置向量更新：更新个体的pbest_i和全局最优gbest；

5)终止条件判断：若达到最大进化代数或满足精度标准，输出全局最优解；否则继续迭代，跳转至步骤3)。

Images