本發(fā)明屬于非線性最優(yōu)估計和地磁測量領(lǐng)域，尤其涉及一種基于地磁矢量及粒子濾波的載體干擾磁場在線補償方法。

背景技術(shù)：

地磁矢量一般使用三軸磁力計進行測量，因此地磁矢量的測量誤差主要分為儀器自身誤差和干擾磁場誤差。由儀器制造工藝和安裝精度產(chǎn)生的儀器誤差是三軸磁力計的自身誤差，磁性物質(zhì)產(chǎn)生的干擾磁場是一種依賴于外界環(huán)境的干擾誤差，兩者都將影響地磁矢量測量的精度。一般來說，載體硬鐵磁場和軟鐵磁場是干擾磁場的主要成分，本發(fā)明只針對硬鐵磁場和軟鐵磁場進行在線補償。硬鐵磁場是由鐵磁性物質(zhì)在外來磁場的作用下被磁化而產(chǎn)生剩磁，剩磁的大小和方向不會隨著載體姿態(tài)的變化而變化。軟鐵磁場與引起它的外加磁場成正比，也隨著載體姿態(tài)的變化而變化。

李季等人使用無跡卡爾曼算法對干擾磁場參數(shù)進行估計，從而對磁測誤差進行補償，但是無跡卡爾曼對非線性較強的系統(tǒng)以及非高斯系統(tǒng)估計性能不佳。一項美國國家專利(美國專利號5182514,1993年1月26日，automaticcompensatorforanairbornemagneticanomalydetector)提到,采用一個三軸磁場傳感器和一個標(biāo)量磁場傳感器，利用兩者輸出計算出地磁場與飛機坐標(biāo)系間的方向余弦；但在干擾磁場強度較大時，存在方向余弦無法測出的問題。

地磁矢量觀測方程具有強的非線性，因此，本發(fā)明采用對非線性具有較好估計效果的粒子濾波算法對載體干擾磁場參數(shù)組成的系統(tǒng)狀態(tài)量進行估計。但是粒子濾波存在隨著迭代次數(shù)的增多有效粒子數(shù)和粒子多樣性都降低的缺陷。本發(fā)明采用雙閾值切割法增加粒子濾波粒子集的有效粒子數(shù)，同時采用bp神經(jīng)網(wǎng)絡(luò)調(diào)整粒子集的權(quán)值，由生物多樣性的熵函數(shù)檢驗粒子集的多樣性，避免了粒子集多樣性的貧乏，實現(xiàn)了載體干擾磁場的在線補償，提高了載體地磁矢量的測量精度。

技術(shù)實現(xiàn)要素：

為了解決干擾磁場對地磁矢量測量精度影響較大的問題，本發(fā)明提出了一種基于地磁矢量及粒子濾波的載體干擾磁場在線補償方法。

本發(fā)明的目的是這樣實現(xiàn)的：

一種基于地磁矢量及粒子濾波的載體干擾磁場在線補償方法，包括如下步驟：

(1)選擇一塊載體干擾磁場補償?shù)膮^(qū)域，用標(biāo)量磁力計測量該區(qū)域的總場值||ho||；

(2)k＝0時刻，對待估參數(shù)進行初始化，根據(jù)先驗概率密度p(x0)產(chǎn)生n個先驗粒子集，所有粒子集的權(quán)值為1/n；

(3)令k＝k+1，載體作改變姿態(tài)的機動動作，由捷聯(lián)于載體的三軸磁力計獲得載體干擾磁場存在時的地磁矢量測量值hmk；

(4)將||hmk||²與||ho||²作差，其差值作為當(dāng)前時刻的系統(tǒng)觀測值yk；式(1)為系統(tǒng)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為待估參數(shù)矩陣，εk為非高斯噪聲；

待估計的硬鐵磁場和軟鐵磁場系數(shù)寫成狀態(tài)向量：

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

式中，硬鐵磁場hpk＝[x1,x2,x3]^t，軟鐵磁場系數(shù)矩陣

其中，

由于載體干擾磁場參數(shù)都是常量，所以粒子濾波的狀態(tài)方程為：

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

式中，ζk為過程噪聲，；

(5)狀態(tài)預(yù)測：根據(jù)狀態(tài)方程從先驗粒子中采樣抽取n個樣本，根據(jù)式(2)計算此時的預(yù)測值yk|k-1；權(quán)值更新：按式(6)進行權(quán)值更新；

先驗概率作為重要性密度函數(shù)：

(6)按式(8)，將n個粒子集權(quán)值進行歸一化處理；

(7)按式(9)，計算有效粒子數(shù)neff1，當(dāng)有效粒子數(shù)neff1≥2n/3時直接進行參數(shù)估計；當(dāng)有效粒子數(shù)neff1＜2n/3時進行步驟(8)；

有效粒子數(shù)的計算公式為：

(8)提高有效粒子數(shù)：選大閾值ω1和小閾值ω2作為粒子集的兩個權(quán)值閾值，選出權(quán)值大于ω1的粒子和權(quán)值低于ω2的粒子；再將大權(quán)值粒子個數(shù)和小權(quán)值粒子個數(shù)進行比較，當(dāng)小權(quán)值粒子個數(shù)是大權(quán)值粒子的n倍時，將大權(quán)值的粒子分割成n個為原來權(quán)值的1/n倍的粒子；之后，用分割后的n個粒子替換小權(quán)值的粒子；經(jīng)過粒子分割替換后的n個粒子，粒子的所有粒子權(quán)值將集中在ω＞ω2區(qū)間，存在粒子多樣性貧乏的問題；

(9)提高粒子多樣性：利用bp神經(jīng)網(wǎng)絡(luò)的非線性特征，權(quán)值大于ω1的粒子的狀態(tài)值用于訓(xùn)練輸入，該時刻的hk值作為訓(xùn)練bp神經(jīng)網(wǎng)絡(luò)的教師信號；然后將權(quán)值低于ω1的粒子的m個狀態(tài)值作為神經(jīng)網(wǎng)絡(luò)的預(yù)測輸入，將此時的輸出值利用權(quán)值更新公式6計算粒子的權(quán)值；將更新后的權(quán)值利用熵值作為權(quán)值多樣性評判函數(shù)，當(dāng)熵d＞0.9m進行步驟10；否則重新進行步驟(9)；

生物多樣性的熵函數(shù)為：

式中，s為更新后的權(quán)值的種類；

(10)對調(diào)整多樣性后的粒子集，再次利用式(8)進行歸一化；

(11)按式(9)計算有效粒子數(shù)neff2，當(dāng)有效粒子數(shù)neff2≥2n/3時直接進行參數(shù)估計；否則進行步驟(12)；

(12)重采樣：將原來的帶權(quán)樣本映射為等權(quán)樣本

(13)參數(shù)估計：利用式(11)進行當(dāng)前時刻的狀態(tài)參數(shù)估計；

(14)將狀態(tài)參數(shù)的估計值代入式(12)，對k時刻的地磁矢量測量值進行實時補償，得到k時刻的地磁矢量補償值；

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

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

本發(fā)明的有益效果在于：本發(fā)明將bp神經(jīng)網(wǎng)絡(luò)、雙閾值切割法和生物多樣性熵函數(shù)應(yīng)用于粒子濾波的參數(shù)估計，增加了粒子濾波的有效粒子數(shù)同時避免了粒子多樣性貧乏的問題，提高載體干擾磁場參數(shù)估計與補償精度。

附圖說明

圖1為本發(fā)明的程序流程示意圖；

圖2為在載體干擾磁場環(huán)境下的地磁矢量測量值；

圖3為硬鐵磁場參數(shù)估計結(jié)果；

圖4為軟鐵磁場系數(shù)矩陣d的主對角線系數(shù)估計結(jié)果；

圖5為軟鐵磁場系數(shù)矩陣d上三角矩陣其余元素的估計結(jié)果；

圖6為補償后的地磁矢量測量值；

圖7為雙閾值切割示意圖；

圖8為bp神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明做進一步描述。

本發(fā)明將bp神經(jīng)網(wǎng)絡(luò)、雙閾值切割法和生物多樣性函數(shù)用于粒子濾波算法的改進，在增加粒子集的有效粒子數(shù)的同時避免了粒子集多樣性貧乏的現(xiàn)象，從而提高了粒子濾波的參數(shù)估計精度，達到地磁矢量測量參數(shù)估計和誤差補償?shù)哪康摹?/p>

步驟1，選擇一塊載體干擾磁場補償?shù)膮^(qū)域，用標(biāo)量磁力計測量該區(qū)域的總場值||ho||，在數(shù)值仿真時設(shè)||ho||＝60000nt。

步驟2，k＝0時刻，對待估參數(shù)進行初始化，根據(jù)先驗概率密度p(x0)產(chǎn)生n個先驗粒子集，所有粒子集的權(quán)值為1/n，將9個參數(shù)的初始值設(shè)為0。

步驟3，令k＝k+1，載體作改變姿態(tài)的機動動作，由捷聯(lián)于載體的三軸磁力計獲得載體干擾磁場存在時的地磁矢量測量值hmk，設(shè)硬鐵磁場hp＝[3000,4000,2000]^t，軟磁場系數(shù)矩陣εk為方差為

2的非高斯噪聲；根據(jù)式(1)仿真得到此時的hmk，hmk的仿真結(jié)果如圖2所示。

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

步驟4，將||hmk||²與||ho||²作差，其差值作為當(dāng)前時刻的系統(tǒng)觀測值yk。式(2)為系統(tǒng)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)

根據(jù)此時的測量值hmk、hpk和b的估計值，求得噪聲νk的均值μk和方差rk。

待估計的載體干擾磁場參數(shù)寫成如下的向量形式：

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

其中，hpk＝[x1,x2,x3]^t；b與d的關(guān)系為：d＝(i3*3+b)^-1-i3×3

i3×3為單位矩陣；由于這些參數(shù)都是常量，所以粒子濾波的狀態(tài)方程為：

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

式中，ζk為過程噪聲。

步驟5，狀態(tài)預(yù)測：根據(jù)狀態(tài)方程從先驗粒子中采樣抽取n個樣本，根據(jù)式(3)計算此時的預(yù)測值yk|k-1；權(quán)值更新：按公式(7)進行權(quán)值更新。

先驗概率作為重要性密度函數(shù)：

具體權(quán)值更新公式為：

步驟6，按式(10)將n個粒子集權(quán)值進行歸一化處理。

步驟7，按式(11)計算有效粒子數(shù)neff1，當(dāng)有效粒子數(shù)neff1≥2n/3時直接進行步驟13；否則，進行步驟8。

步驟8，提高有效粒子數(shù)：選大閾值ω1和小閾值ω2作為粒子集的兩個權(quán)值閾值，選出權(quán)值大于ω1的粒子和權(quán)值低于ω2的粒子；再將大權(quán)值粒子個數(shù)和小權(quán)值粒子個數(shù)進行比較，當(dāng)小權(quán)值粒子個數(shù)是大權(quán)值粒子的n倍時，將大權(quán)值的粒子分割成n個為原來權(quán)值的1/n倍的粒子；之后，用分割后的n個粒子替換小權(quán)值的粒子。經(jīng)過粒子分割替換后的n個粒子，粒子的所有粒子權(quán)值將集中在ω＞ω2區(qū)間，存在粒子多樣性貧乏的問題。具體分割方式見圖7。

假設(shè)40m個粒子分布為：經(jīng)上述雙閾值切割和替換后的粒子分布為全部粒子的權(quán)值將大于小閾值ω2，有效粒子數(shù)明顯提高。但是由于經(jīng)平均切割后的粒子出現(xiàn)較多權(quán)值相同的粒子，存在粒子多樣性貧乏的現(xiàn)象。

步驟9，提高粒子多樣性：利用bp神經(jīng)網(wǎng)絡(luò)的非線性特征，權(quán)值大于ω1的粒子的狀態(tài)值用于訓(xùn)練輸入，該時刻的hk值作為訓(xùn)練bp神經(jīng)網(wǎng)絡(luò)的教師信號；然后

將權(quán)值低于ω1的粒子的m個狀態(tài)值作為神經(jīng)網(wǎng)絡(luò)的預(yù)測輸入，將此時的輸出值利用權(quán)值更新公式(9)計算粒子的權(quán)值；將更新后的權(quán)值利用熵值作為權(quán)值多樣性評判函數(shù)，當(dāng)熵d＞0.9m進行步驟10；否則重新進行步驟9。

生物多樣性的熵函數(shù)為：

式中，s為更新后的權(quán)值的種類。

用bp神經(jīng)網(wǎng)絡(luò)和生物多樣性熵值來提高粒子的多樣性，具體步驟如下：

a.建立一個三層的神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)：由于待估參數(shù)是9個，所以將神經(jīng)網(wǎng)絡(luò)輸入神

經(jīng)元設(shè)為9維，輸出神經(jīng)元的個數(shù)為1；根據(jù)經(jīng)驗公式，隱層神經(jīng)元的個數(shù)l1為輸入神經(jīng)元個數(shù)，l2為輸出神經(jīng)元個數(shù)，因此隱層神經(jīng)元為3個。神經(jīng)網(wǎng)絡(luò)的具體結(jié)構(gòu)見附圖8。

b.隱含層激活函數(shù)為tansig，輸出層的函數(shù)為purelin；

c.訓(xùn)練好網(wǎng)絡(luò)后，用小權(quán)值的狀態(tài)做預(yù)測輸入，得到相應(yīng)的輸出值，再根據(jù)此時的觀測值用公式(9)得到對應(yīng)的權(quán)值，然后用多樣性熵函數(shù)檢測粒子的多樣性；

步驟10，對調(diào)整多樣性后的粒子集，再次利用式(10)進行歸一化。

步驟11，按式(11)計算有效粒子數(shù)neff2，當(dāng)有效粒子數(shù)neff2≥2n/3時直接進

行參數(shù)估計；否則進行步驟12。

步驟12，重采樣：將原來的帶權(quán)樣本映射為等權(quán)樣本

步驟13，參數(shù)估計：利用式(13)進行當(dāng)前時刻的狀態(tài)參數(shù)估計。

步驟14，將狀態(tài)參數(shù)的估計值代入式(14)，對k時刻的地磁矢量測量值進行實時補償，得到k時刻的地磁矢量補償值。

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

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