一种基于地磁矢量及粒子滤波的载体干扰磁场在线补偿方法与流程

本发明属于非线性最优估计和地磁测量领域，尤其涉及一种基于地磁矢量及粒子滤波的载体干扰磁场在线补偿方法。

背景技术：

地磁矢量一般使用三轴磁力计进行测量，因此地磁矢量的测量误差主要分为仪器自身误差和干扰磁场误差。由仪器制造工艺和安装精度产生的仪器误差是三轴磁力计的自身误差，磁性物质产生的干扰磁场是一种依赖于外界环境的干扰误差，两者都将影响地磁矢量测量的精度。一般来说，载体硬铁磁场和软铁磁场是干扰磁场的主要成分，本发明只针对硬铁磁场和软铁磁场进行在线补偿。硬铁磁场是由铁磁性物质在外来磁场的作用下被磁化而产生剩磁，剩磁的大小和方向不会随着载体姿态的变化而变化。软铁磁场与引起它的外加磁场成正比，也随着载体姿态的变化而变化。

李季等人使用无迹卡尔曼算法对干扰磁场参数进行估计，从而对磁测误差进行补偿，但是无迹卡尔曼对非线性较强的系统以及非高斯系统估计性能不佳。一项美国国家专利(美国专利号5182514,1993年1月26日，automaticcompensatorforanairbornemagneticanomalydetector)提到,采用一个三轴磁场传感器和一个标量磁场传感器，利用两者输出计算出地磁场与飞机坐标系间的方向余弦；但在干扰磁场强度较大时，存在方向余弦无法测出的问题。

地磁矢量观测方程具有强的非线性，因此，本发明采用对非线性具有较好估计效果的粒子滤波算法对载体干扰磁场参数组成的系统状态量进行估计。但是粒子滤波存在随着迭代次数的增多有效粒子数和粒子多样性都降低的缺陷。本发明采用双阈值切割法增加粒子滤波粒子集的有效粒子数，同时采用bp神经网络调整粒子集的权值，由生物多样性的熵函数检验粒子集的多样性，避免了粒子集多样性的贫乏，实现了载体干扰磁场的在线补偿，提高了载体地磁矢量的测量精度。

技术实现要素：

为了解决干扰磁场对地磁矢量测量精度影响较大的问题，本发明提出了一种基于地磁矢量及粒子滤波的载体干扰磁场在线补偿方法。

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

一种基于地磁矢量及粒子滤波的载体干扰磁场在线补偿方法，包括如下步骤：

(1)选择一块载体干扰磁场补偿的区域，用标量磁力计测量该区域的总场值||ho||；

(2)k＝0时刻，对待估参数进行初始化，根据先验概率密度p(x0)产生n个先验粒子集，所有粒子集的权值为1/n；

(3)令k＝k+1，载体作改变姿态的机动动作，由捷联于载体的三轴磁力计获得载体干扰磁场存在时的地磁矢量测量值hmk；

(4)将||hmk||²与||ho||²作差，其差值作为当前时刻的系统观测值yk；式(1)为系统k时刻的观测方程：

yk＝||hmk||²-||ho||²＝hk+νk(1)

式中,

hk＝2h'pkhmk-||hpk||²-(hmk-hpk)'(b+b'+b'b)(hmk-hpk)(2)

νk＝2(hmk-hpk)'(i3×3+b+b'+b'b)εk-εk'(i3×3+b+b'+b'b)εk(3)

式中，i3×3为单位矩阵，b为待估参数矩阵，εk为非高斯噪声；

待估计的硬铁磁场和软铁磁场系数写成状态向量：

x＝[x1,x2,x3,x4,x5,x6,x7,x8,x9]^t(4)

式中，硬铁磁场hpk＝[x1,x2,x3]^t，软铁磁场系数矩阵

其中，

由于载体干扰磁场参数都是常量，所以粒子滤波的状态方程为：

x(k)＝x(k-1)+ζk(5)

式中，ζk为过程噪声，；

(5)状态预测：根据状态方程从先验粒子中采样抽取n个样本，根据式(2)计算此时的预测值yk|k-1；权值更新：按式(6)进行权值更新；

先验概率作为重要性密度函数：

(6)按式(8)，将n个粒子集权值进行归一化处理；

(7)按式(9)，计算有效粒子数neff1，当有效粒子数neff1≥2n/3时直接进行参数估计；当有效粒子数neff1＜2n/3时进行步骤(8)；

有效粒子数的计算公式为：

(8)提高有效粒子数：选大阈值ω1和小阈值ω2作为粒子集的两个权值阈值，选出权值大于ω1的粒子和权值低于ω2的粒子；再将大权值粒子个数和小权值粒子个数进行比较，当小权值粒子个数是大权值粒子的n倍时，将大权值的粒子分割成n个为原来权值的1/n倍的粒子；之后，用分割后的n个粒子替换小权值的粒子；经过粒子分割替换后的n个粒子，粒子的所有粒子权值将集中在ω＞ω2区间，存在粒子多样性贫乏的问题；

(9)提高粒子多样性：利用bp神经网络的非线性特征，权值大于ω1的粒子的状态值用于训练输入，该时刻的hk值作为训练bp神经网络的教师信号；然后将权值低于ω1的粒子的m个状态值作为神经网络的预测输入，将此时的输出值利用权值更新公式6计算粒子的权值；将更新后的权值利用熵值作为权值多样性评判函数，当熵d＞0.9m进行步骤10；否则重新进行步骤(9)；

生物多样性的熵函数为：

式中，s为更新后的权值的种类；

(10)对调整多样性后的粒子集，再次利用式(8)进行归一化；

(11)按式(9)计算有效粒子数neff2，当有效粒子数neff2≥2n/3时直接进行参数估计；否则进行步骤(12)；

(12)重采样：将原来的带权样本映射为等权样本

(13)参数估计：利用式(11)进行当前时刻的状态参数估计；

(14)将状态参数的估计值代入式(12)，对k时刻的地磁矢量测量值进行实时补偿，得到k时刻的地磁矢量补偿值；

hmkb＝(i+b)(hmk-hp)(12)

(15)k＝k+1，回到步骤(3)；

本发明的有益效果在于：本发明将bp神经网络、双阈值切割法和生物多样性熵函数应用于粒子滤波的参数估计，增加了粒子滤波的有效粒子数同时避免了粒子多样性贫乏的问题，提高载体干扰磁场参数估计与补偿精度。

附图说明

图1为本发明的程序流程示意图；

图2为在载体干扰磁场环境下的地磁矢量测量值；

图3为硬铁磁场参数估计结果；

图4为软铁磁场系数矩阵d的主对角线系数估计结果；

图5为软铁磁场系数矩阵d上三角矩阵其余元素的估计结果；

图6为补偿后的地磁矢量测量值；

图7为双阈值切割示意图；

图8为bp神经网络结构示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明将bp神经网络、双阈值切割法和生物多样性函数用于粒子滤波算法的改进，在增加粒子集的有效粒子数的同时避免了粒子集多样性贫乏的现象，从而提高了粒子滤波的参数估计精度，达到地磁矢量测量参数估计和误差补偿的目的。

步骤1，选择一块载体干扰磁场补偿的区域，用标量磁力计测量该区域的总场值||ho||，在数值仿真时设||ho||＝60000nt。

步骤2，k＝0时刻，对待估参数进行初始化，根据先验概率密度p(x0)产生n个先验粒子集，所有粒子集的权值为1/n，将9个参数的初始值设为0。

步骤3，令k＝k+1，载体作改变姿态的机动动作，由捷联于载体的三轴磁力计获得载体干扰磁场存在时的地磁矢量测量值hmk，设硬铁磁场hp＝[3000,4000,2000]^t，软磁场系数矩阵εk为方差为

2的非高斯噪声；根据式(1)仿真得到此时的hmk，hmk的仿真结果如图2所示。

hmk＝hok+hp+hi+εk＝(i3*3+d)hok+hp+εk(1)

步骤4，将||hmk||²与||ho||²作差，其差值作为当前时刻的系统观测值yk。式(2)为系统k时刻的观测方程：

yk＝||hmk||²-||ho||²＝hk+νk(2)

式中：

hk＝2h'pkhmk-||hpk||²-(hmk-hpk)'(b+b'+b'b)(hmk-hpk)(3)

νk＝2(hmk-hpk)'(i3×3+b+b'+b'b)εk-εk'(i3×3+b+b'+b'b)εk(4)

根据此时的测量值hmk、hpk和b的估计值，求得噪声νk的均值μk和方差rk。

待估计的载体干扰磁场参数写成如下的向量形式：

x＝[x1,x2,x3,x4,x5,x6,x7,x8,x9]^t(5)

其中，hpk＝[x1,x2,x3]^t；b与d的关系为：d＝(i3*3+b)^-1-i3×3

i3×3为单位矩阵；由于这些参数都是常量，所以粒子滤波的状态方程为：

x(k)＝x(k-1)+ζk(6)

式中，ζk为过程噪声。

步骤5，状态预测：根据状态方程从先验粒子中采样抽取n个样本，根据式(3)计算此时的预测值yk|k-1；权值更新：按公式(7)进行权值更新。

先验概率作为重要性密度函数：

具体权值更新公式为：

步骤6，按式(10)将n个粒子集权值进行归一化处理。

步骤7，按式(11)计算有效粒子数neff1，当有效粒子数neff1≥2n/3时直接进行步骤13；否则，进行步骤8。

步骤8，提高有效粒子数：选大阈值ω1和小阈值ω2作为粒子集的两个权值阈值，选出权值大于ω1的粒子和权值低于ω2的粒子；再将大权值粒子个数和小权值粒子个数进行比较，当小权值粒子个数是大权值粒子的n倍时，将大权值的粒子分割成n个为原来权值的1/n倍的粒子；之后，用分割后的n个粒子替换小权值的粒子。经过粒子分割替换后的n个粒子，粒子的所有粒子权值将集中在ω＞ω2区间，存在粒子多样性贫乏的问题。具体分割方式见图7。

假设40m个粒子分布为：经上述双阈值切割和替换后的粒子分布为全部粒子的权值将大于小阈值ω2，有效粒子数明显提高。但是由于经平均切割后的粒子出现较多权值相同的粒子，存在粒子多样性贫乏的现象。

步骤9，提高粒子多样性：利用bp神经网络的非线性特征，权值大于ω1的粒子的状态值用于训练输入，该时刻的hk值作为训练bp神经网络的教师信号；然后

将权值低于ω1的粒子的m个状态值作为神经网络的预测输入，将此时的输出值利用权值更新公式(9)计算粒子的权值；将更新后的权值利用熵值作为权值多样性评判函数，当熵d＞0.9m进行步骤10；否则重新进行步骤9。

生物多样性的熵函数为：

式中，s为更新后的权值的种类。

用bp神经网络和生物多样性熵值来提高粒子的多样性，具体步骤如下：

a.建立一个三层的神经网络结构：由于待估参数是9个，所以将神经网络输入神

经元设为9维，输出神经元的个数为1；根据经验公式，隐层神经元的个数l1为输入神经元个数，l2为输出神经元个数，因此隐层神经元为3个。神经网络的具体结构见附图8。

b.隐含层激活函数为tansig，输出层的函数为purelin；

c.训练好网络后，用小权值的状态做预测输入，得到相应的输出值，再根据此时的观测值用公式(9)得到对应的权值，然后用多样性熵函数检测粒子的多样性；

步骤10，对调整多样性后的粒子集，再次利用式(10)进行归一化。

步骤11，按式(11)计算有效粒子数neff2，当有效粒子数neff2≥2n/3时直接进

行参数估计；否则进行步骤12。

步骤12，重采样：将原来的带权样本映射为等权样本

步骤13，参数估计：利用式(13)进行当前时刻的状态参数估计。

步骤14，将状态参数的估计值代入式(14)，对k时刻的地磁矢量测量值进行实时补偿，得到k时刻的地磁矢量补偿值。

hmkb＝(i+b)(hmk-hp)(14)

步骤15，k＝k+1，回到步骤3。