對(duì)水聲定位濾波算法記憶衰減方式的改進(jìn)

馬建業(yè) ，鄭東健

(1.河海大學(xué) 水利水電學(xué)院，江蘇 南京 210098；2.河海大學(xué) 水資源高效利用與工程安全國家工程研究中心，江蘇 南京 210098；3.河海大學(xué) 水文水資源與水利工程科學(xué)國家重點(diǎn)實(shí)驗(yàn)室，江蘇 南京 210098)

水工建筑物的安全監(jiān)測(cè)工作有助于把握工程的工作狀態(tài)，評(píng)估其安全程度[1]。在智慧水利[2]的基本框架中，建筑物的安全監(jiān)測(cè)工作處于基礎(chǔ)的感知層[3]，由于水工建筑物工作環(huán)境的特殊性，有很大一部分長(zhǎng)期位于水下，安全監(jiān)測(cè)工作存在諸多困難。自動(dòng)化的安全監(jiān)測(cè)設(shè)備不僅有利于提高監(jiān)測(cè)工作的安全性和便捷性，實(shí)時(shí)掌握水工建筑物的安全狀態(tài)，而且有利于對(duì)運(yùn)行管理提出指導(dǎo)性建議[4]。監(jiān)測(cè)設(shè)備的水下定位具有基礎(chǔ)性和關(guān)鍵性的作用，對(duì)水下定位算法的研究有利于提高水下建筑物安全監(jiān)測(cè)工作的準(zhǔn)確性和便捷性。

無線電磁波和光波等會(huì)在水下迅速衰減，無法在水下定位系統(tǒng)中使用。聲波在水下可以遠(yuǎn)距離傳播，是水下信息傳播的主要載體[5]，水下定位大多為水下聲學(xué)定位。在通信、導(dǎo)航、目標(biāo)跟蹤等過程中，不可避免地存在著由干擾因素引起的誤差，例如水下聲學(xué)定位中的水聲環(huán)境噪聲干擾誤差、量測(cè)誤差及系統(tǒng)誤差等[6-7]。為在一定程度上抑制干擾噪聲還原真實(shí)數(shù)據(jù)，數(shù)據(jù)濾波往往是目標(biāo)定位系統(tǒng)中的重要組成部分。在眾多的濾波算法中，最小二乘法（Least squares,LS）和卡爾曼濾波算法是應(yīng)用較為廣泛的兩種經(jīng)典算法[8]。其中，卡爾曼濾波因其能對(duì)現(xiàn)場(chǎng)采集數(shù)據(jù)的實(shí)時(shí)更新、便于計(jì)算機(jī)編程及其特有的數(shù)據(jù)融合功能等優(yōu)勢(shì)應(yīng)用最為廣泛，被認(rèn)為是最佳的線性濾波器。在解決水聲定位等非線性問題時(shí)，往往應(yīng)用其非線性形式：擴(kuò)展卡爾曼濾波（Extended Kalman Filtering,EKF）和無跡卡爾曼濾波[9]。傳統(tǒng)卡爾曼濾波有一明顯缺陷，即不準(zhǔn)確的系統(tǒng)數(shù)學(xué)模型和噪聲統(tǒng)計(jì)模型及由于計(jì)算機(jī)計(jì)算長(zhǎng)度有限或是方法本身的近似處理而產(chǎn)生的誤差會(huì)進(jìn)行積累，從而使得誤差矩陣失去正定性甚至對(duì)稱性，導(dǎo)致濾波算法無法達(dá)到理論上的收斂。邱愷等[10]根據(jù)所提出的卡爾曼濾波過程的穩(wěn)定性對(duì)濾波發(fā)散現(xiàn)象進(jìn)行了較為清晰的定義。衰減記憶卡爾曼濾波[11]是解決此類問題的一個(gè)重要手段，衰減因子對(duì)濾波算法的性能影響較大，不適宜的衰減因子不僅對(duì)發(fā)散現(xiàn)象沒有明顯的抑制作用，而且還會(huì)加劇濾波算法的發(fā)散。關(guān)于衰減因子的取值，目前有兩種常見的方法：一是傳統(tǒng)的選取方法，即根據(jù)實(shí)際情況選取一個(gè)定值[12]，對(duì)環(huán)境的變化缺乏適應(yīng)性，且受主觀因素影響較大；二是采用自適應(yīng)衰減因子[13]，使衰減因子在每一步的預(yù)測(cè)中都處于變化之中，這在一定程度上提高了衰減因子選取的靈活性。自適應(yīng)衰減因子的確定主要有兩種形式：函數(shù)型[14]和概率型[15-16]。自適應(yīng)衰減因子雖提高了因子選取的靈活性，但仍需要主觀確定全部或部分參數(shù)，且對(duì)濾波過程是否需要進(jìn)行記憶衰減的判斷未作出深入探討。針對(duì)此問題，本文對(duì)水聲定位算法中的擴(kuò)展卡爾曼濾波進(jìn)行了改進(jìn)，其核心是在對(duì)濾波算法是否發(fā)散進(jìn)行判定的基礎(chǔ)上，以觀測(cè)值為標(biāo)準(zhǔn)對(duì)記憶進(jìn)行衰減。

1 算法描述

1.1 水聲定位算法

傳統(tǒng)水下目標(biāo)定位幾何算法可分為3 類[17]，本文選擇較為簡(jiǎn)單的基于信號(hào)到達(dá)時(shí)間的（Time of Arrive,TOA）水聲定位算法。如圖1 所示，設(shè)觀測(cè)節(jié)點(diǎn)A、B、C的坐標(biāo)分別為（x1,y1）、（x2,y2）、（x3,y3）。定位目標(biāo)T的坐標(biāo)設(shè)為（x,y）。T與3 個(gè)觀測(cè)節(jié)點(diǎn)之間的距離由所記錄的信號(hào)到達(dá)時(shí)間計(jì)算：

圖1 目標(biāo)定位算法示意Fig.1 Schematic diagram of positioning algorithm

式中：c為信號(hào)傳播速度；ti為信號(hào)到達(dá)各觀測(cè)節(jié)點(diǎn)的傳輸時(shí)間。

TOA 水聲定位算法的基本原理如下所述，在定位目標(biāo)T與觀測(cè)節(jié)點(diǎn)A、B、C時(shí)間同步的條件下，移動(dòng)目標(biāo)節(jié)點(diǎn)T作為信號(hào)源發(fā)射測(cè)量信號(hào)的同時(shí)，發(fā)送一個(gè)同步信號(hào)給各觀測(cè)節(jié)點(diǎn)，觀測(cè)節(jié)點(diǎn)通過記錄信號(hào)到達(dá)時(shí)間，計(jì)算定位目標(biāo)與觀測(cè)節(jié)點(diǎn)的距離。

根據(jù)幾何關(guān)系，T的位置坐標(biāo)可由式（2）計(jì)算：

式中：ri為目標(biāo)節(jié)點(diǎn)與各觀測(cè)節(jié)點(diǎn)之間的距離。

1.2 改進(jìn)的衰減記憶卡爾曼濾波算法

在整個(gè)水聲定位過程中，目標(biāo)的真實(shí)位置是不可知的，無法以此為標(biāo)準(zhǔn)作出是否需要進(jìn)行記憶衰減的判斷。觀測(cè)值理論上包含了真實(shí)位置的信息，可以用于判斷濾波是否存在異常的發(fā)散現(xiàn)象。對(duì)衰減記憶卡爾曼濾波算法進(jìn)行了改進(jìn)，式（3）是本文對(duì)算法改進(jìn)的數(shù)學(xué)表達(dá)。

式中：λ為衰減因子；‖?‖為矩陣范數(shù)，在本式中其具體涵義為取矩陣各元素絕對(duì)值的最大值，可以較為容易地驗(yàn)證此定義下的矩陣范數(shù)滿足正定性、齊次性和三角不等式；f表示以括號(hào)中的表達(dá)式為目標(biāo)，對(duì)衰減因子進(jìn)行尋優(yōu)處理；H為轉(zhuǎn)移矩陣，其作用是將狀態(tài)域中的各元素轉(zhuǎn)換到觀測(cè)域參與計(jì)算；M為時(shí)間點(diǎn)；w’為預(yù)測(cè)模型的等效標(biāo)準(zhǔn)差，是根據(jù)觀測(cè)方程式（2）和預(yù)測(cè)模型標(biāo)準(zhǔn)差w，并依據(jù)一般函數(shù)的誤差傳播定律求得的；v為觀測(cè)模型的標(biāo)準(zhǔn)差；β為儲(chǔ)備系數(shù)；式中其余字母含義與傳統(tǒng)衰減記憶卡爾曼濾波算法中的定義一致。

如式（3）所示，本文對(duì)傳統(tǒng)算法的改進(jìn)主要包括：作出是否需要進(jìn)行記憶衰減的判斷；以觀測(cè)值為標(biāo)準(zhǔn)，對(duì)記憶進(jìn)行衰減。本文考慮到傳統(tǒng)濾波發(fā)散判定方法的局限性[18]，采取以濾波過程穩(wěn)定性[10]為基礎(chǔ)的判斷標(biāo)準(zhǔn)，該判斷標(biāo)準(zhǔn)可以合理剔除正常的發(fā)散，如由于初始值設(shè)置不準(zhǔn)確而導(dǎo)致的局部發(fā)散和觀測(cè)野值等。

2 水下目標(biāo)跟蹤仿真試驗(yàn)

2.1 系統(tǒng)模型

本文以TOA 水聲定位算法為例（圖1），在平面區(qū)域進(jìn)行仿真試驗(yàn)，3 個(gè)觀測(cè)節(jié)點(diǎn)呈等間距的L 型布置，間隔為d=50 m。定位目標(biāo)在運(yùn)動(dòng)過程中周期性地發(fā)出聲信號(hào)，觀測(cè)節(jié)點(diǎn)根據(jù)接收到的信號(hào)對(duì)定位目標(biāo)的位置進(jìn)行計(jì)算。

設(shè)3 個(gè)觀測(cè)節(jié)點(diǎn)的坐標(biāo)分別為A(0,0)、B(0,d)、C(d,0)，定位目標(biāo)的坐標(biāo)為T(x,y)。根據(jù)式（2）所述的定位原理，可得定位目標(biāo)的坐標(biāo)如式（4）：

式中：t1、t2、t3分別對(duì)應(yīng)A、B、C接受目標(biāo)信號(hào)的傳播時(shí)間。

仿真試驗(yàn)的結(jié)果分析和比較標(biāo)準(zhǔn)采用式（5）所示的定位目標(biāo)預(yù)測(cè)狀態(tài)向量與實(shí)際狀態(tài)向量之間的均方根誤差表示。

2.2 參數(shù)選定

對(duì)濾波過程中參數(shù)的合理選定，不僅能夠使仿真試驗(yàn)更加接近真實(shí)信號(hào)，增加仿真試驗(yàn)結(jié)果的可信度，還會(huì)對(duì)濾波效果產(chǎn)生明顯影響，不合理的參數(shù)甚至?xí)?dǎo)致濾波發(fā)散。文獻(xiàn)[19]討論了影響擴(kuò)展卡爾曼濾波的4 個(gè)因素，并給出了其選擇原則。

被定位目標(biāo)的初始狀態(tài)X0對(duì)濾波效果有較大影響，合理的初始狀態(tài)可以減小定位誤差，加快濾波收斂，最好控制相對(duì)誤差在12%以內(nèi)。狀態(tài)的初始噪聲協(xié)方差矩陣P對(duì)濾波效果影響較小，可任意取一個(gè)非零矩陣，如單位對(duì)角矩陣。模型本身的噪聲協(xié)方差矩陣Q取值越小越好。對(duì)于觀測(cè)噪聲的協(xié)方差矩陣R，工程上一般是在濾波前先測(cè)定噪聲協(xié)方差，然后再用于后續(xù)的濾波。

根據(jù)文獻(xiàn)[20]所述，短基線水聲定位過程中，測(cè)量值的誤差通常指?jìng)鬏敃r(shí)間誤差，且具有隨機(jī)性，脈沖前沿測(cè)量誤差和時(shí)鐘誤差共同構(gòu)成了時(shí)延誤差，時(shí)鐘誤差可以通過精度較高的設(shè)備進(jìn)行校對(duì)來避免，可忽略不計(jì)；而脈沖前沿的測(cè)量誤差與信噪比相關(guān)，根據(jù)誤差理論可知，由信噪比引起的時(shí)延誤差符合統(tǒng)計(jì)規(guī)律，服從均值為0 的正態(tài)分布。

文獻(xiàn)[7]提出了計(jì)算時(shí)延均方差的明確公式：

式中：W為可接收的信號(hào)帶寬；Sn為信噪比（dB）。

根據(jù)1 kHz～1 MHz 標(biāo)準(zhǔn)水聽器的行業(yè)標(biāo)準(zhǔn)[21]所述：“在實(shí)際測(cè)量條件下檢測(cè)信噪比，其值應(yīng)不小于30 dB”，而中心頻率為200 kHz 的DYW-200-G 水聲換能器的工作帶寬為10 kHz。綜合以上信息，本文選定時(shí)延均方差的數(shù)量級(jí)在10-4和10-6之間。

理論分析得出的模型本身的噪聲和觀測(cè)噪聲均為高斯白噪聲，這個(gè)條件在實(shí)際工程未必得到滿足，并且各類環(huán)境誤差和系統(tǒng)誤差也是不可能完全被消除的。這些在濾波過程中未考慮到或是設(shè)定不準(zhǔn)確的噪聲誤差可能會(huì)隨濾波過程而逐漸積累，這也是需要引入衰減記憶濾波算法的一個(gè)重要原因。根據(jù)文獻(xiàn)[7]對(duì)主流短基線水聲定位系統(tǒng)的精度分析，其定位誤差一般不超過3 m。故除常規(guī)噪聲設(shè)置外，為仿真模擬系統(tǒng)數(shù)學(xué)模型不準(zhǔn)確的情況，在狀態(tài)轉(zhuǎn)移過程中增加模型變化矩陣Bt。

式中：Xt為t時(shí)刻的真實(shí)狀態(tài)；Ft為t時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣。

2.3 仿真試驗(yàn)工況設(shè)置

仿真試驗(yàn)中涉及3 種不同的工況：（1）理想工況。初始狀態(tài)X0、狀態(tài)轉(zhuǎn)移矩陣F、狀態(tài)初始噪聲P、模型本身噪聲Q和觀測(cè)噪聲R等5 個(gè)參數(shù)均已準(zhǔn)確設(shè)置。（2）實(shí)際工況。系統(tǒng)數(shù)學(xué)模型設(shè)置不夠準(zhǔn)確，存在一定的定位誤差，且誤差在主流水聲定位系統(tǒng)的精度范圍之內(nèi)。（3）異常工況。初始狀態(tài)X0設(shè)置不合理，與實(shí)際狀態(tài)的相對(duì)誤差超過12%，觀測(cè)值存在一定的野值。

3 種工況下的模型本身噪聲Q和觀測(cè)噪聲R，均分別設(shè)置為Q=diag(0.012m2)，R=diag(0.0022m2)，其中，diag 表示對(duì)角矩陣，其后括號(hào)里的數(shù)值為方差，矩陣維數(shù)分別與各自模型相適應(yīng)。系統(tǒng)數(shù)學(xué)模型的不準(zhǔn)確通過模型變化矩陣進(jìn)行仿真。其他仿真參數(shù)設(shè)置如下：（1）理想工況。初始狀態(tài)X0=S0（S0為定位目標(biāo)實(shí)際初始狀態(tài)），狀態(tài)轉(zhuǎn)移矩陣F準(zhǔn)確，模型變化矩陣Bt為0。（2）實(shí)際工況。初始狀態(tài)X0中各元素與S0（S0為定位目標(biāo)實(shí)際初始狀態(tài)）中各元素的相對(duì)誤差均小于12%，狀態(tài)轉(zhuǎn)移矩陣F不準(zhǔn)確，模型變化矩陣Bt非0，且標(biāo)準(zhǔn)差不超過3 m。（3）異常工況。初始狀態(tài)X0中各元素與S0中各元素的相對(duì)誤差均大于12%，狀態(tài)轉(zhuǎn)移矩陣F準(zhǔn)確。觀測(cè)值在預(yù)測(cè)過程中的個(gè)別時(shí)間點(diǎn)存在野值。

3 仿真試驗(yàn)結(jié)果與討論

3.1 衰減記憶方法的合理性驗(yàn)證

實(shí)際情況下，由于目標(biāo)真實(shí)狀態(tài)無法獲取，故無法根據(jù)目標(biāo)狀態(tài)真實(shí)值與最終預(yù)測(cè)值之間的均方根誤差 δ1判別系統(tǒng)是否存在發(fā)散趨勢(shì)。故本文認(rèn)為狀態(tài)的初步預(yù)測(cè)值與觀測(cè)值所反映的狀態(tài)之間的均方根殘差δ2一定程度上可以反映目標(biāo)狀態(tài)真實(shí)值與最終預(yù)測(cè)值之間的 δ1變化趨勢(shì)，可據(jù)此來選擇是否需要進(jìn)行記憶衰減。

式中：si為目標(biāo)各狀態(tài)分量的真實(shí)值；為目標(biāo)狀態(tài)最終預(yù)測(cè)值；zi為觀測(cè)值；為目標(biāo)狀態(tài)初步預(yù)測(cè)值在觀測(cè)域的各等效分量，由觀測(cè)方程計(jì)算得到。

根據(jù)實(shí)際工況，選取預(yù)測(cè)模型噪聲協(xié)方差矩陣Q=diag(0.012m2)，觀測(cè)模型標(biāo)準(zhǔn)差R=diag(0.022m2)，預(yù)測(cè)模型變化矩陣為均值為0 m、標(biāo)準(zhǔn)差為2.5 m 的隨機(jī)矩陣，在此條件下進(jìn)行水下目標(biāo)定位仿真試驗(yàn)，記錄δ1和δ2，兩者趨勢(shì)關(guān)系在圖2 中得以體現(xiàn)。

從圖2 中可以看出，兩類均方根誤差的變化趨勢(shì)基本相同，且 δ2對(duì) δ1有一定程度上的放大效果。試驗(yàn)結(jié)果表明，可根據(jù)目標(biāo)狀態(tài)的初步預(yù)測(cè)值與觀測(cè)值之間的離散程度來判斷系統(tǒng)是否有發(fā)散趨勢(shì)，并以此作為是否需要進(jìn)行記憶衰減的判斷依據(jù)。作為是否需要進(jìn)行記憶衰減的判斷依據(jù)，δ2的大小可能與衰減因子具體數(shù)值的選取具有一定的相關(guān)性。并且衰減因子的引入主要是為了解決由誤差累積所引起的系統(tǒng)發(fā)散問題，從這個(gè)角度分析，衰減因子還可能與預(yù)測(cè)時(shí)長(zhǎng)具有一定的相關(guān)性，已有研究成果也利用了此種關(guān)系[14]。

圖2 均方根誤差變化趨勢(shì)對(duì)比Fig.2 Comparison of RMSE change trends

為此，本文在預(yù)測(cè)模型噪聲協(xié)方差矩陣為Q=diag(0.012m2)，觀測(cè)模型協(xié)方差矩陣為R=diag(0.022m2)，預(yù)測(cè)模型變化矩陣的均值為0 m、方差為0.05 m2的隨機(jī)矩陣的條件下，進(jìn)行水聲定位仿真實(shí)驗(yàn)，記錄目標(biāo)狀態(tài)初步預(yù)測(cè)值與觀測(cè)值之間的殘差，并經(jīng)過歸一化處理后，求得 δ2。根據(jù)目標(biāo)狀態(tài)真實(shí)值與最終預(yù)測(cè)值之間的均方根誤差最小的原則，進(jìn)行衰減因子的尋優(yōu)，并對(duì)試驗(yàn)結(jié)果進(jìn)行了相關(guān)分析。

計(jì)算結(jié)果表明，最優(yōu)衰減因子與兩個(gè)變量之間的雙尾顯著性分別為-0.053 和0.186，其絕對(duì)值均大于0.05，這表明顯著性不高；且相關(guān)系數(shù)分別為0.275 和-0.190（表1），其絕對(duì)值均小于0.3，這表明無相關(guān)性。故本試驗(yàn)中，難以用確切的函數(shù)表達(dá)式將 δ2和預(yù)測(cè)時(shí)長(zhǎng)兩個(gè)變量與衰減因子聯(lián)系起來。

表1 衰減因子相關(guān)分析Tab.1 Correlation analysis of attenuation factors

綜合分析以上兩部分的試驗(yàn)結(jié)果，本文提出如式（3）所示的衰減因子選取的改進(jìn)算法，其主要步驟是根據(jù)目標(biāo)狀態(tài)的初步預(yù)測(cè)值與觀測(cè)值之間的離散程度，對(duì)濾波算法作出是否有發(fā)散趨勢(shì)的判定后，以觀測(cè)值為標(biāo)準(zhǔn)對(duì)記憶進(jìn)行衰減。

3.2 對(duì)比試驗(yàn)

為驗(yàn)證改進(jìn)的算法（Improved fading memory Kalman filter algorithm,IFMKF）性能，本文在3 種工況條件下，在水聲定位仿真試驗(yàn)中，對(duì)其與傳統(tǒng)衰減記憶濾波卡爾曼濾波算法（Traditional fading memory Kalman filter algorithm,TFMKF）和指數(shù)型加權(quán)自適應(yīng)衰減記憶卡爾曼濾波算法（Exponential weighted adaptive fading memory Kalman filter algorithm,EWFMKF）的定位精度進(jìn)行比較。TFMKF 算法中的衰減因子，根據(jù)文獻(xiàn)[12]的研究結(jié)果，選定為1.23。EWFMKF 中參數(shù)b，按照文獻(xiàn)[14]所述，取0 到1 之間的數(shù)值。分別記錄水下目標(biāo)跟蹤過程中定位過程的坐標(biāo)位置均方根誤差。

圖3 是理想工況下的3 種算法的均方根誤差比較。在水聲定位中，當(dāng)各類參數(shù)均準(zhǔn)確或接近準(zhǔn)確設(shè)置時(shí)，水聲定位工況接近理想工況，濾波過程穩(wěn)定，沒有發(fā)散現(xiàn)象，無需進(jìn)行記憶衰減。從圖3 可見：3 種定位算法在30 s 之前的均方根誤差大小及變化趨勢(shì)基本相同，但在之后的定位過程中，由于TFMKF 和EWFMKF 兩種濾波算法進(jìn)行了記憶衰減，反而對(duì)定位精度產(chǎn)生了負(fù)面影響。從表2 中可見IFMKF 的累計(jì)定位均方根誤差均小于TFMKF 和EWFMKF，定位精度分別相對(duì)提高19.4%和21.4%。

圖3 理想工況下3 種算法的均方根誤差比較Fig.3 RMSE comparison of the three algorithms under ideal conditions

圖4 是實(shí)際工況下的3 種算法的均方根誤差比較。在水聲定位中，當(dāng)各類參數(shù)均欠準(zhǔn)確或不準(zhǔn)確設(shè)置時(shí)，水聲定位工況接近實(shí)際工況，濾波過程不穩(wěn)定；隨著濾波過程的推進(jìn)，誤差會(huì)進(jìn)行累積，造成定位精度的發(fā)散，屬于異常發(fā)散，需要進(jìn)行記憶衰減。本文在此種工況下，設(shè)定系統(tǒng)狀態(tài)轉(zhuǎn)移模型設(shè)置不夠準(zhǔn)確，模型變化矩陣Bt的元素為1～2 m 間均勻分布的隨機(jī)數(shù)。初始狀態(tài)設(shè)置也與實(shí)際狀態(tài)存在6%的相對(duì)誤差。從圖4 可以看出，3 種算法在水聲定位過程中的均方根誤差變化趨勢(shì)基本相同，大小存在差異。從表2 可見，IFMKF 的累計(jì)定位均方根誤差均小于TFMKF 和EWFMKF，定位精度分別相對(duì)提高78.4%和48.8%。由于仿真試驗(yàn)中設(shè)定系統(tǒng)狀態(tài)轉(zhuǎn)移的數(shù)學(xué)模型不準(zhǔn)確，而觀測(cè)模型準(zhǔn)確設(shè)置，故可能會(huì)導(dǎo)致定位精度提高百分比較大，但精度提高的整體趨勢(shì)不變。

圖4 實(shí)際工況下3 種算法的均方根誤差比較Fig.4 Comparison of RMSE of the three algorithms under actual working conditions

表2 不同記憶衰減方法的累積定位均方根誤差Tab.2 Cumulative root mean square error of different memory attenuation methods

圖5 和6 是異常工況下3 種算法的均方根誤差比較。在水聲定位中，除初始狀態(tài)外，當(dāng)各類參數(shù)準(zhǔn)確或近似準(zhǔn)確設(shè)置，且觀測(cè)值存在個(gè)別野值時(shí)，水聲定位工況接近異常工況；雖然在個(gè)別時(shí)間內(nèi)，定位結(jié)果與實(shí)際位置存在較大誤差，但濾波過程穩(wěn)定；隨著濾波過程的推進(jìn)，誤差不會(huì)進(jìn)行累積，不需要進(jìn)行記憶衰減。本文在此種工況下，設(shè)定系統(tǒng)初始狀態(tài)與實(shí)際狀態(tài)存在20%的相對(duì)誤差，在15 s 時(shí)刻，觀測(cè)值出現(xiàn)一個(gè)異常的野值。

圖5 異常工況下3 種算法觀測(cè)野值出現(xiàn)前的均方根誤差比較Fig.5 Comparison of the RMSE of the three algorithms before the occurrence of outliers under abnormal conditions

從圖5 可以看出：在野值出現(xiàn)前，由于初始狀態(tài)的不準(zhǔn)確設(shè)定，定位誤差在初始的6 s 內(nèi)出現(xiàn)了小幅度的增大，但又很快得到了抑制，屬于正常的發(fā)散現(xiàn)象，不需要進(jìn)行記憶衰減；而3 種算法在這段時(shí)間內(nèi)的定位精度及變化趨勢(shì)基本相同。

從圖6 可以看出：在野值出現(xiàn)之后，TFMKF 和EWFMKF 兩種濾波算法與IFMKF 相比使得定位誤差陡然升高，對(duì)水聲定位精度產(chǎn)生了負(fù)面影響。需要剔除野值，不應(yīng)以此判斷濾波發(fā)散。相較于其他兩種方法，由于IFMKF 以整個(gè)濾波過程為判斷依據(jù)，故在有觀測(cè)野值的工況下，定位誤差沒有急劇上升，定位效果更優(yōu)。由于觀測(cè)野值出現(xiàn)后，定位誤差的陡然升高，故定位精度不具有可比性，因此異常工況試驗(yàn)條件下的試驗(yàn)結(jié)果在表2 中不再進(jìn)行精度比較。

圖6 異常工況下3 種算法的均方根誤差比較Fig.6 RMSE comparison of the three algorithms under abnormal conditions

4 結(jié)語

本文對(duì)水聲定位中的擴(kuò)展卡爾曼濾波算法進(jìn)行了改進(jìn)，為水聲定位中的擴(kuò)展卡爾曼濾波算法的記憶衰減方式提供了一種新的思路和方法。改進(jìn)算法以定位過程中觀測(cè)值與初步預(yù)測(cè)值之間的差值為判斷依據(jù)，以濾波過程的穩(wěn)定性為判斷原則，作出是否需要進(jìn)行記憶衰減的判斷后，以觀測(cè)值為標(biāo)準(zhǔn)進(jìn)行記憶衰減。將改進(jìn)的算法應(yīng)用于3 種不同工況的水聲定位仿真試驗(yàn)中。仿真結(jié)果表明，較TFMKF 和EWFMKF 兩種算法，改進(jìn)算法的記憶衰減方式更為準(zhǔn)確和靈活，能有效避免對(duì)算法是否需要進(jìn)行記憶衰減的誤判，定位效果和定位精度得到了提高。