本發(fā)明屬于通信技術(shù)領(lǐng)域，尤其涉及一種基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法。

背景技術(shù)：

無(wú)線傳感器網(wǎng)絡(luò)(Wireless Sensor Networks，WSNs)作為計(jì)算、通信和傳感器技術(shù)相結(jié)合的產(chǎn)物，改變了人類與自然界的交互方式，將邏輯上的信息世界與客觀上的物理世界融合在一起，提升了人類認(rèn)識(shí)世界、改造世界的能力。在WSNs的應(yīng)用領(lǐng)域中，不含有節(jié)點(diǎn)位置的感知信息不會(huì)產(chǎn)生任何價(jià)值。為了順利完成監(jiān)測(cè)任務(wù)，隨機(jī)散布的傳感器節(jié)點(diǎn)需要具備及時(shí)準(zhǔn)確提供自身位置信息的能力，這對(duì)整個(gè)系統(tǒng)功能的實(shí)現(xiàn)至關(guān)重要。把節(jié)點(diǎn)定位方法按照是否需要測(cè)距進(jìn)行劃分是最常用的一種分類方法，即基于測(cè)距(Range-based)和測(cè)距無(wú)關(guān)(Range-free)的定位方法。Range-based的定位方法假定節(jié)點(diǎn)能運(yùn)用幾何關(guān)系測(cè)量相互間的距離或方向，并利用這個(gè)距離或方向信息作為約束確定節(jié)點(diǎn)的位置，其中基于信號(hào)接收強(qiáng)度指示(RSSI)的測(cè)距方法被廣泛應(yīng)用于節(jié)點(diǎn)定位中。而Range-free的方法則無(wú)需測(cè)量節(jié)點(diǎn)間的距離和方向，僅僅利用網(wǎng)絡(luò)拓?fù)涞男畔?例如節(jié)點(diǎn)的鄰居關(guān)系或節(jié)點(diǎn)間的跳數(shù)等)來(lái)確定節(jié)點(diǎn)的位置，由于傳感器節(jié)點(diǎn)在部署中難以做到均勻分布，再加上無(wú)線信號(hào)沿各方向傳播的不規(guī)則性，使得這類定位方法的誤差較大。以目前看來(lái)，盡管行之有效的定位算法層出不窮，仍有很多問(wèn)題亟待解決：1)廣泛使用的RSSI定位方法僅需要少量的信號(hào)采樣值進(jìn)行測(cè)距，然后同時(shí)對(duì)信道參數(shù)與節(jié)點(diǎn)位置進(jìn)行估計(jì)。然而這類方法通常具有一個(gè)固定前提，即假設(shè)發(fā)射信號(hào)功率恒定不變，但是實(shí)際上發(fā)射信號(hào)的功率不能總保持恒定，因?yàn)橐紤]到工作電源的變化、器件參數(shù)不同及環(huán)境擾動(dòng)等因素影響，這就使得RSSI定位方法受到局限。2)很多基于測(cè)距的方法都是利用無(wú)線電傳播損耗模型測(cè)距，然而由于實(shí)際環(huán)境不同，模型中的參數(shù)也不同。雖然基于非線性濾波的RSSI定位方法能夠利用RSSI值對(duì)模型中的參數(shù)進(jìn)行動(dòng)態(tài)估計(jì)，但同時(shí)估計(jì)的節(jié)點(diǎn)位置精度并不十分讓人滿意，需要對(duì)此類方法作進(jìn)一步的改進(jìn)。近年來(lái)，非線性狀態(tài)估計(jì)越來(lái)越發(fā)揮出了重要的作用，其在目標(biāo)跟蹤、信息處理、參數(shù)估計(jì)及定位等方面都具有重要的應(yīng)用。先后發(fā)展了擴(kuò)展卡爾曼濾波(ExtendedKalman Filter,EKF)、容積卡爾曼濾波(Cubature Kalman Filter,CKF)和無(wú)跡卡爾曼濾波(Unscented Kalman Filter,UKF)等多種非線性濾波方法，然而這些衍生的卡爾曼濾波方法對(duì)于非高斯模型來(lái)說(shuō)均不是最優(yōu)的濾波器。粒子濾波是另一類非線性濾波方法，與衍生的卡爾曼濾波方法相比，系統(tǒng)的非線性和非高斯性越強(qiáng)，粒子濾波效果越好。但是針對(duì)高斯非線性模型，有人采用球面徑向容積準(zhǔn)則的積分法則進(jìn)行系統(tǒng)狀態(tài)的非線性傳遞的CKF算法，不僅有效克服了EKF在強(qiáng)非線性系統(tǒng)中的應(yīng)用局限性，且濾波精度甚至高于粒子濾波。為了提高CKF的性能，在其濾波過(guò)程中加入誤差協(xié)方差陣的平方根，用來(lái)保證CKF誤差協(xié)方差陣的對(duì)稱性和正(半)定性，從而構(gòu)成了平方根容積卡爾曼濾波(Square-root Cubature KalmanFilter,SCKF)算法。SCKF算法是一種免微分的濾波算法，其以CKF為框架，使用容積原則進(jìn)行相應(yīng)的數(shù)值積分來(lái)處理狀態(tài)均值和協(xié)方差的非線性傳遞，濾波精度高，所需容積點(diǎn)個(gè)數(shù)較少，計(jì)算量小且運(yùn)算時(shí)間較短。有人提出的基于SCKF的RSSI狀態(tài)估計(jì)算法，將RSSI定位問(wèn)題轉(zhuǎn)變?yōu)榉蔷€性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題，利用SCKF對(duì)WSNs中待定位節(jié)點(diǎn)位置和RSSI信道衰減系數(shù)及距離信源1m處的接收信號(hào)功率同時(shí)進(jìn)行估計(jì)，并利用參數(shù)動(dòng)態(tài)變化實(shí)時(shí)修正估計(jì)的定位節(jié)點(diǎn)坐標(biāo)，進(jìn)一步提高定位精度。SCKF算法不具有對(duì)測(cè)量條件變化和系統(tǒng)模型不確定性的自適應(yīng)性，不良的測(cè)量或系統(tǒng)模型的變化會(huì)影響濾波性能甚至導(dǎo)致濾波故障，其對(duì)不良測(cè)量的魯棒性可以通過(guò)應(yīng)用新息協(xié)方差匹配技術(shù)建立自適應(yīng)濾波、魯棒濾波得到提高，其對(duì)模型不確定的影響可以利用強(qiáng)跟蹤濾波器來(lái)緩解，但STF具有計(jì)算精度低及需要計(jì)算雅克比矩陣等理論局限性?？紤]受到噪聲的影響，特別在低信噪比情況下，SCKF的濾波結(jié)果會(huì)出現(xiàn)發(fā)散情況，其性能會(huì)受到限制。

綜上所述，現(xiàn)有的非線性系統(tǒng)狀態(tài)估計(jì)方法存在均方根誤差較大、狀態(tài)估計(jì)不準(zhǔn)確的問(wèn)題。

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

本發(fā)明的目的在于提供一種基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法，旨在解決現(xiàn)有的非線性系統(tǒng)狀態(tài)估計(jì)方法存在均方根誤差較大、狀態(tài)估計(jì)不準(zhǔn)確的問(wèn)題。

本發(fā)明是這樣實(shí)現(xiàn)的，一種基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法，所述基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法提出了一種普通非線性濾波算法與卡爾曼濾波相結(jié)合的強(qiáng)適應(yīng)卡爾曼濾波機(jī)制，即在SCKF或EKF的狀態(tài)向量估計(jì)基礎(chǔ)上利用卡爾曼濾波再進(jìn)行處理，從而使得估計(jì)的結(jié)果得到改善。該機(jī)制首先對(duì)非線性模型利用SCKF進(jìn)行容積近似，得到狀態(tài)向量的估計(jì)值，又依據(jù)狀態(tài)方程的線性變化，使用卡爾曼濾波進(jìn)一步處理得到最優(yōu)估計(jì)，建立強(qiáng)適應(yīng)SCKF算法。為對(duì)比分析，同理給出了強(qiáng)適應(yīng)EKF算法的設(shè)計(jì)步驟以及系統(tǒng)模型下基于RSSI狀態(tài)向量估計(jì)的理論均方根誤差下界。

進(jìn)一步，所述平方根容積卡爾曼濾波算法的基礎(chǔ)上利用卡爾曼濾波進(jìn)行處理的方法包括：

步驟一，初始化濾波，定義系統(tǒng)在k＝0時(shí)刻狀態(tài)向量的初值為其協(xié)方差矩陣為P_0|0，通過(guò)QR分解得到方差陣的平方根S_0|0，模擬生成待定位節(jié)點(diǎn)接收到的n個(gè)通信范圍內(nèi)的信標(biāo)節(jié)點(diǎn)的RSSI值；

步驟二，循環(huán)執(zhí)行平方根容積卡爾曼濾波算法，F(xiàn)or k＝1,2,…N，N為取樣次數(shù)，將和S_k|k作為輸入，重復(fù)執(zhí)行直到N次取樣完成；由平方根容積卡爾曼濾波算法得到的估計(jì)值與之間的誤差應(yīng)該符合一個(gè)加性噪聲的關(guān)系，即其中觀測(cè)噪聲e_k假設(shè)服從均值為零，協(xié)方差矩陣為Φ_k＝cov(e_k)的高斯分布；

綜上得到如下動(dòng)態(tài)系統(tǒng)，其狀態(tài)空間模型為：

狀態(tài)方程:

測(cè)量方程:

進(jìn)一步，所述步驟二包括：

(1)按照平方根容積卡爾曼濾波算法，進(jìn)行時(shí)間更新和測(cè)量更新得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值及相應(yīng)的誤差協(xié)方差陣的平方根S_k|k；

(2)執(zhí)行卡爾曼濾波遞推更新

在平方根容積卡爾曼濾波算法估計(jì)的基礎(chǔ)上進(jìn)行進(jìn)一步卡爾曼濾波，得到最優(yōu)估計(jì)

P_k|k-1＝AP_k-1|k-1A^T+Q_k-1；

G_k＝P_k|k-1(Φ_k+P_k|k-1)^-1；

P_k|k＝(I-G_k)P_k|k-1；

其中P_k-1|k-1為k-1時(shí)刻最優(yōu)估計(jì)值的協(xié)方差矩陣，Q_k-1為過(guò)程噪聲w_k-1的協(xié)方差矩陣P_k|k-1為k時(shí)刻預(yù)測(cè)值的協(xié)方差矩陣，P_k|k為k時(shí)刻最優(yōu)估計(jì)值的協(xié)方差矩陣，G_k是卡爾曼增益。關(guān)于初值的設(shè)置，令為平方根容積卡爾曼濾波算法估計(jì)前狀態(tài)向量的初值，P_0|0為平方根容積卡爾曼濾波算法估計(jì)前協(xié)方差矩陣的初值；

得到的協(xié)方差矩陣P_k|k-1，卡爾曼增益G_k，得到狀態(tài)向量的最優(yōu)估計(jì)得到的協(xié)方差矩陣P_k|k；

(3)計(jì)算狀態(tài)向量理論上的均方根誤差下界，對(duì)于v＝[v₁,v₂,…v_n]^T，其中v_i是待定位節(jié)點(diǎn)接收第i個(gè)信標(biāo)節(jié)點(diǎn)的RSSI時(shí)對(duì)應(yīng)的隨機(jī)測(cè)量噪聲，且即均方根誤差下界通過(guò)克拉默-拉奧界獲得如下：

其中F通過(guò)下式來(lái)計(jì)算：

進(jìn)一步，所述擴(kuò)展卡爾曼濾波的基礎(chǔ)上利用卡爾曼濾波進(jìn)行處理的方法包括：

(1)初始化濾波，定義系統(tǒng)在k＝0時(shí)刻狀態(tài)向量的初值為其協(xié)方差矩陣初值為C_0|0，卡爾曼濾波狀態(tài)向量的協(xié)方差初值為P_0|0，模擬生成待定位節(jié)點(diǎn)接收到的n個(gè)通信范圍內(nèi)的信標(biāo)節(jié)點(diǎn)的RSSI值；

(2)循環(huán)執(zhí)行擴(kuò)展卡爾曼濾波

For k＝1,2,…N，N為取樣次數(shù)，將得到的和C_k|k作為輸入，重復(fù)執(zhí)行直到N次取樣完成；

在狀態(tài)空間模型中，對(duì)于待估計(jì)的狀態(tài)向量由于系統(tǒng)方程γ_k＝Aγ_k-1+w_k-1中的A是單位狀態(tài)轉(zhuǎn)移矩陣，無(wú)需求導(dǎo)；測(cè)量方程中的非線性觀測(cè)函數(shù)h(γ_k)＝[h₁(γ_k),h₂(γ_k),…h(huán)_n(γ_k)]^T，其中1≤i≤n；對(duì)此函數(shù)在γ_k處求偏導(dǎo)數(shù)得到如下Jacobian矩陣H_k：

進(jìn)一步，所述(2)包括：

按照下述公式，得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值及相應(yīng)的誤差協(xié)方差陣C_k|k：

C_k|k-1＝AC_k-1|k-1A^T+Q_k-1

C_k|k＝(I-W_kH_k)C_k|k-1

執(zhí)行卡爾曼濾波更新：

得到的協(xié)方差矩陣P_k|k-1，得到卡爾曼增益G_k，得到狀態(tài)向量的最優(yōu)估計(jì)得到的協(xié)方差矩陣P_k|k。

本發(fā)明的另一目的在于提供一種利用所述基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法的無(wú)線傳感器網(wǎng)絡(luò)。

本發(fā)明提供的基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法，比起現(xiàn)有的未經(jīng)強(qiáng)適應(yīng)遞推的非線性濾波算法以及單一的僅適用于線性系統(tǒng)模型的卡爾曼濾波算法，強(qiáng)適應(yīng)濾波設(shè)計(jì)使得系統(tǒng)的狀態(tài)估計(jì)取得了更低的均方根誤差，相對(duì)于理論均方根誤差下界甚至?xí)@得更低的誤差下界。本發(fā)明不過(guò)分依賴于不合適的初始條件，具有較好的算法穩(wěn)定性，能夠很好地適用于高度非線性化系統(tǒng)，不會(huì)輕易讓算法發(fā)散失效，計(jì)算量小且運(yùn)算時(shí)間較短，而且提出的強(qiáng)適應(yīng)機(jī)制是建立在系統(tǒng)狀態(tài)模型、觀測(cè)模型及噪聲都符合高斯分布假設(shè)的基礎(chǔ)上，所以將來(lái)的研究工作可以拓展到非高斯背景下的非線性狀態(tài)估計(jì)研究。

在SCKF或EKF的狀態(tài)向量估計(jì)基礎(chǔ)上利用卡爾曼濾波再進(jìn)行處理，從而使得估計(jì)的結(jié)果得到改善，提高精度。在50次獨(dú)立仿真試驗(yàn)中，狀態(tài)向量的前兩個(gè)分量即位置的平均累加均方根誤差分別由0.3279和0.1572降低到0.1886和0.0901，下降幅度為0.1393和0.0671，提高了定位精度。另外狀態(tài)向量每個(gè)分量的累加均方根誤差分別為(0.2716,0.1838,0.3014,2.0909)、(0.1509,0.1131,0.2261,1.6196)、(0.1109,0.1114,0.1162,0.6843)和(0.0620,0.0654,0.0704,0.4047)，而全部分量的累加均方根誤差則分別降低了0.4917和0.2910，明顯有所下降。由此可見(jiàn)，強(qiáng)適應(yīng)濾波設(shè)計(jì)使得非線性動(dòng)態(tài)系統(tǒng)的狀態(tài)估計(jì)達(dá)到了更低的均方根誤差，取得了更好的估計(jì)效果。

附圖說(shuō)明

圖1是本發(fā)明實(shí)施例提供的基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法流程圖。

圖2是本發(fā)明實(shí)施例提供的實(shí)施例1的總流程圖。

圖3是本發(fā)明實(shí)施例提供的隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)位置估計(jì)過(guò)程的比較圖。

圖4是本發(fā)明實(shí)施例提供的隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)位置均方根誤差的比較圖。

圖5是本發(fā)明實(shí)施例提供的隨著取樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)狀態(tài)估計(jì)的比較圖。

圖6是本發(fā)明實(shí)施例提供的隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)狀態(tài)估計(jì)均方根誤差的比較圖。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

下面結(jié)合附圖對(duì)本發(fā)明的應(yīng)用原理作詳細(xì)的描述。

如圖1所示，本發(fā)明實(shí)施例提供的基于卡爾曼濾波定位的非線性系統(tǒng)狀態(tài)估計(jì)方法包括以下步驟：

S101：根據(jù)無(wú)線信道衰減模型建立狀態(tài)空間模型，將定位問(wèn)題轉(zhuǎn)化為非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題；

S102：使用基于平方根容積卡爾曼濾波(SCKF)的RSSI狀態(tài)估計(jì)算法，對(duì)節(jié)點(diǎn)位置和信道參數(shù)同時(shí)進(jìn)行估計(jì)，得到狀態(tài)向量的估計(jì)值；

S103：依據(jù)狀態(tài)方程的線性變化，使用卡爾曼濾波進(jìn)一步處理得到最優(yōu)估計(jì)；

S104：給出強(qiáng)適應(yīng)擴(kuò)展卡爾曼濾波(EKF)算法的設(shè)計(jì)步驟；

S105：對(duì)系統(tǒng)狀態(tài)空間模型下基于RSSI狀態(tài)估計(jì)的理論均方根誤差下界進(jìn)行分析。

下面結(jié)合具體實(shí)施例對(duì)本發(fā)明的應(yīng)用原理作進(jìn)一步的描述。

實(shí)施例1：

參照?qǐng)D2，針對(duì)SCKF算法的強(qiáng)適應(yīng)卡爾曼濾波設(shè)計(jì)發(fā)明的實(shí)現(xiàn)步驟如下：

步驟1.初始化濾波

定義系統(tǒng)在k＝0時(shí)刻狀態(tài)向量的初值為其協(xié)方差矩陣為P_0|0，通過(guò)QR分解得到方差陣的平方根S_0|0，模擬生成待定位節(jié)點(diǎn)接收到的n個(gè)通信范圍內(nèi)的信標(biāo)節(jié)點(diǎn)的RSSI值；

步驟2.循環(huán)執(zhí)行SCKF

For k＝1,2,…N，N為取樣次數(shù)，將步驟2.2中得到的和步驟2.1中得到的S_k|k作為輸入，重復(fù)執(zhí)行步驟2.1和2.2直到N次取樣完成。

由式(5)，考慮到k時(shí)刻待定位節(jié)點(diǎn)的狀態(tài)向量滿足這一順序估計(jì)，此時(shí)由SCKF得到的估計(jì)值與之間的誤差應(yīng)該符合一個(gè)加性噪聲的關(guān)系，即其中觀測(cè)噪聲e_k假設(shè)服從均值為零，協(xié)方差矩陣為Φ_k＝cov(e_k)的高斯分布。

綜上得到如下動(dòng)態(tài)系統(tǒng)，其狀態(tài)空間模型為：

狀態(tài)方程:

測(cè)量方程:

步驟2.1.按照SCKF算法設(shè)計(jì)，進(jìn)行時(shí)間更新和測(cè)量更新得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值及相應(yīng)的誤差協(xié)方差陣的平方根S_k|k；

步驟2.2.執(zhí)行卡爾曼濾波遞推更新

對(duì)于線性高斯模型，卡爾曼濾波器被公認(rèn)為是最優(yōu)濾波器，因此在SCKF估計(jì)的基礎(chǔ)上進(jìn)行進(jìn)一步卡爾曼濾波，得到最優(yōu)估計(jì)

P_k|k-1＝AP_k-1|k-1A^T+Q_k-1 (9)

G_k＝P_k|k-1(Φ_k+P_k|k-1)^-1 (10)

P_k|k＝(I-G_k)P_k|k-1 (12)

其中P_k-1|k-1為k-1時(shí)刻最優(yōu)估計(jì)值的協(xié)方差矩陣，Q_k-1為過(guò)程噪聲w_k-1的協(xié)方差矩陣P_k|k-1為k時(shí)刻預(yù)測(cè)值(因?yàn)锳為單位狀態(tài)轉(zhuǎn)移矩陣，故預(yù)測(cè)值與前一時(shí)刻的最優(yōu)估計(jì)值相同)的協(xié)方差矩陣，P_k|k為k時(shí)刻最優(yōu)估計(jì)值的協(xié)方差矩陣，G_k是卡爾曼增益。關(guān)于初值的設(shè)置，令為SCKF估計(jì)前狀態(tài)向量的初值，P_0|0為SCKF估計(jì)前協(xié)方差矩陣的初值。

由式(9)得到的協(xié)方差矩陣P_k|k-1，由式(10)得到卡爾曼增益G_k，由式(11)得到狀態(tài)向量的最優(yōu)估計(jì)由式(12)得到的協(xié)方差矩陣P_k|k。

步驟2.3.計(jì)算狀態(tài)向量理論上的均方根誤差下界

本發(fā)明所提的強(qiáng)適應(yīng)濾波算法對(duì)狀態(tài)向量γ_k的估計(jì)收斂于最優(yōu)估計(jì)，為分析算法性能優(yōu)劣，將的估計(jì)方差與均方根誤差下界進(jìn)行對(duì)比。對(duì)于v＝[v₁,v₂,…v_n]^T，其中v_i是待定位節(jié)點(diǎn)接收第i個(gè)信標(biāo)節(jié)點(diǎn)的RSSI時(shí)對(duì)應(yīng)的隨機(jī)測(cè)量噪聲，且即對(duì)于由系統(tǒng)模型轉(zhuǎn)化成的固定參數(shù)無(wú)偏估計(jì)問(wèn)題，理論上的均方根誤差下界可以通過(guò)克拉默-拉奧界(CRB)獲得如下：

其中F可通過(guò)下式來(lái)計(jì)算：

參照?qǐng)D2，針對(duì)EKF算法的強(qiáng)適應(yīng)卡爾曼濾波設(shè)計(jì)發(fā)明的實(shí)現(xiàn)步驟如下：

步驟1.初始化濾波

定義系統(tǒng)在k＝0時(shí)刻狀態(tài)向量的初值為其協(xié)方差矩陣初值為C_0|0，卡爾曼濾波狀態(tài)向量的協(xié)方差初值為P_0|0，模擬生成待定位節(jié)點(diǎn)接收到的n個(gè)通信范圍內(nèi)的信標(biāo)節(jié)點(diǎn)的RSSI值；

步驟2.循環(huán)執(zhí)行EKF

For k＝1,2,…N，N為取樣次數(shù)，將步驟2.2中得到的和步驟2.1中得到的C_k|k作為輸入，重復(fù)執(zhí)行步驟2.1和2.2直到N次取樣完成。

在狀態(tài)空間模型中，對(duì)于待估計(jì)的狀態(tài)向量由于系統(tǒng)方程γ_k＝Aγ_k-1+w_k-1中的A是單位狀態(tài)轉(zhuǎn)移矩陣，無(wú)需求導(dǎo)。測(cè)量方程中的非線性觀測(cè)函數(shù)h(γ_k)＝[h₁(γ_k),h₂(γ_k),…h(huán)_n(γ_k)]^T，其中1≤i≤n。對(duì)此函數(shù)在γ_k處求偏導(dǎo)數(shù)得到如下Jacobian矩陣H_k：

步驟2.1.執(zhí)行EKF

按照下述公式設(shè)計(jì)，得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值及相應(yīng)的誤差協(xié)方差陣C_k|k：

C_k|k-1＝AC_k-1|k-1A^T+Q_k-1

C_k|k＝(I-W_kH_k)C_k|k-1

步驟2.2.執(zhí)行卡爾曼濾波更新

由式(9)得到的協(xié)方差矩陣P_k|k-1，由式(10)得到卡爾曼增益G_k，由式(13)得到狀態(tài)向量的最優(yōu)估計(jì)由式(12)得到的協(xié)方差矩陣P_k|k。

本發(fā)明的效果可以通過(guò)以下仿真實(shí)驗(yàn)結(jié)果進(jìn)行進(jìn)一步的說(shuō)明。

1.仿真條件：

為了驗(yàn)證算法的有效性，在Matlab條件下對(duì)本發(fā)明算法進(jìn)行了仿真實(shí)驗(yàn)，該算法的仿真參數(shù)為：

該發(fā)明算法的評(píng)價(jià)指標(biāo)定義如下：

1)待定位節(jié)點(diǎn)在k時(shí)刻的位置均方根誤差：

其中為k時(shí)刻待定位節(jié)點(diǎn)橫坐標(biāo)x_k的估計(jì)值，為縱坐標(biāo)y_k的估計(jì)值。

2)待定位節(jié)點(diǎn)的位置累加均方根誤差：

3)狀態(tài)向量中每個(gè)分量的累加均方根誤差：

4)狀態(tài)向量中全部分量的累加均方根誤差：

2.仿真內(nèi)容及仿真結(jié)果：

表2：本發(fā)明和其他算法關(guān)于狀態(tài)向量估計(jì)過(guò)程的累加均方根誤差比較表；

圖3：隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)位置估計(jì)過(guò)程的比較圖；

圖4：隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)位置均方根誤差的比較圖；

圖5：隨著取樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)狀態(tài)估計(jì)的比較圖；

圖6：隨著采樣次數(shù)的變化，本發(fā)明和其他算法的節(jié)點(diǎn)狀態(tài)估計(jì)均方根誤差的比較圖；

仿真1，給出了各算法的累加均方根誤差對(duì)比結(jié)果，如表2所示，由表2看出，強(qiáng)適應(yīng)濾波設(shè)計(jì)在原本的非線性濾波算法，即SCKF和EKF的基礎(chǔ)上，使得非線性系統(tǒng)的狀態(tài)向量估計(jì)問(wèn)題取得了更低的均方根誤差，達(dá)到了更高的估計(jì)精度，由此可見(jiàn)，此類設(shè)計(jì)可行且高效。

仿真2，針對(duì)待定位節(jié)點(diǎn)的位置估計(jì)過(guò)程，分別給出了SCKF和強(qiáng)適應(yīng)SCKF、EKF和強(qiáng)適應(yīng)EKF這四種濾波算法的對(duì)比結(jié)果，仿真如圖3所示。從圖3中可以看到，明顯強(qiáng)適應(yīng)算法估計(jì)坐標(biāo)的收斂性優(yōu)于未經(jīng)強(qiáng)適應(yīng)遞推的普通算法。

仿真3，在卡爾曼濾波的N次數(shù)據(jù)采樣過(guò)程中(也即1s時(shí)長(zhǎng)內(nèi))，分別給出了四種不同濾波算法的位置均方根誤差曲線，仿真如圖4所示。從圖4中可以看到，對(duì)于待定位節(jié)點(diǎn)位置坐標(biāo)的估計(jì)，強(qiáng)適應(yīng)濾波算法在原算法的基礎(chǔ)上取得了更低的誤差且具有較好的穩(wěn)定性。

仿真4，隨著濾波0.1s的間隔采樣，分別對(duì)狀態(tài)空間模型中節(jié)點(diǎn)狀態(tài)向量的每一個(gè)分量的狀態(tài)估計(jì)值進(jìn)行了對(duì)比，仿真結(jié)果如圖5所示。從圖5可以看出，這四種算法均能夠漸進(jìn)收斂到節(jié)點(diǎn)狀態(tài)向量分量的真實(shí)值，SCKF與EKF算法的估計(jì)結(jié)果不夠穩(wěn)定，會(huì)在真實(shí)值上下浮動(dòng)，而強(qiáng)適應(yīng)SCKF與EKF算法由于是對(duì)原來(lái)算法的進(jìn)一步濾波，結(jié)果看起來(lái)更精確可靠。而且強(qiáng)適應(yīng)濾波算法對(duì)待定位節(jié)點(diǎn)的橫縱坐標(biāo)估計(jì)更加準(zhǔn)確，而對(duì)信道衰減系數(shù)及距離信源1m處的RSSI值估計(jì)不夠理想。但是不可否認(rèn)的是，SCKF算法雖然利用信道參數(shù)反饋修正節(jié)點(diǎn)位置坐標(biāo)，在室內(nèi)定位情形下仍然沒(méi)能達(dá)到和EKF一樣好的狀態(tài)估計(jì)精度。

仿真5，給出了對(duì)應(yīng)于仿真3中節(jié)點(diǎn)狀態(tài)估計(jì)的均方根誤差對(duì)比曲線圖，并與理論上的均方根誤差下界CRB進(jìn)行對(duì)比，仿真結(jié)果如圖6所示。從圖6可以看出，隨著取樣次數(shù)的增加，四種算法基本都接近誤差下界，但強(qiáng)適應(yīng)濾波算法會(huì)獲得更低的誤差下界，這是由于CRB下界的推導(dǎo)根據(jù)的是式(3-5)和式(3-6)所示的非線性模型，其只能作為SCKF和EKF的誤差下界，而強(qiáng)適應(yīng)濾波算法是在SCKF和EKF的基礎(chǔ)上進(jìn)行的線性遞推估計(jì)，能夠進(jìn)一步消除噪聲的影響，其向量估計(jì)誤差可與CRB對(duì)比分析。

以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。