非線性測量系統(tǒng)狀態(tài)估計(jì)算法介紹目錄TOC\o"1-3"\h\u8709非線性測量系統(tǒng)狀態(tài)估計(jì)算法介紹 1131011.1非線性測量系統(tǒng)模型 172451.2濾波算法 2274721.2.1擴(kuò)展卡爾曼濾波算法 2324051.2.2無跡卡爾曼 496151.3回歸評估標(biāo)準(zhǔn) 91.1非線性測量系統(tǒng)模型非線性系統(tǒng)狀態(tài)空間模型是一種常見的描述非線性過程的模型，非線性系統(tǒng)模型由兩個(gè)方程確定，一個(gè)為狀態(tài)方程，一個(gè)為測量方程。系統(tǒng)的測量方程由于傳感器本身精度問題再加上模型自身噪聲及環(huán)境干擾，一般不能將測量工具傳出的信號(hào)當(dāng)作簡單的線性疊加來計(jì)算。本文將考慮這樣一個(gè)非線性測量系統(tǒng)：該系統(tǒng)的觀測方程為非線性，狀態(tài)方程為線性，并且噪聲的概率密度函數(shù)服從高斯分布。非線性系統(tǒng)的狀態(tài)空間方程如下： (1.1) (1.2)式中：為狀態(tài)向量；為輸入向量；為輸出向量；為狀態(tài)轉(zhuǎn)移函數(shù)；為測量函數(shù)；為系統(tǒng)噪聲，其協(xié)方差矩陣為；為觀測噪聲，其協(xié)方差矩陣為；與是零均值的高斯噪聲，且相互獨(dú)立。下面是非線性狀態(tài)空間模型：圖1.SEQ圖\*ARABIC1-4非線性狀態(tài)空間模型圖本文將考慮一個(gè)目標(biāo)物體做勻速直線運(yùn)動(dòng)的系統(tǒng)，其中有存在隨機(jī)擾動(dòng)作為加速度，這里將很多物理因素的共同作用歸結(jié)為數(shù)學(xué)模型中的隨機(jī)擾動(dòng)。只設(shè)置了單個(gè)且固定不動(dòng)的觀測站，對運(yùn)動(dòng)目標(biāo)進(jìn)行觀測。1.2濾波算法1.2.1擴(kuò)展卡爾曼濾波算法擴(kuò)展卡爾曼濾波在卡爾曼濾波的基礎(chǔ)上，通過將狀態(tài)方程和觀測方程分別圍繞上一步濾波值和上一步預(yù)測值進(jìn)行一階泰勒展開的方式將非線性系統(tǒng)線性化，從而達(dá)到可以對非線性系統(tǒng)進(jìn)行狀態(tài)估計(jì)的功能REF_Ref11920\n\h[17]。圖1.2-5擴(kuò)展卡爾曼濾波算法框圖擴(kuò)展卡爾曼濾波算法（EKF）的主要思想REF_Ref11920\n\h[22]是在每個(gè)時(shí)間步長對系統(tǒng)進(jìn)行線性化處理，從而將非線性系統(tǒng)轉(zhuǎn)化為近似非線性系統(tǒng)的線性時(shí)變系統(tǒng)，然后將卡爾曼濾波器應(yīng)用在線性時(shí)變系統(tǒng)中。線性化系統(tǒng)狀態(tài)方程和觀測方程可以表示為：擴(kuò)展卡爾曼濾波的狀態(tài)轉(zhuǎn)移方程和觀測方程為： (1.5) (1.6)其中過程噪聲W(k)和觀測噪聲V(k)相互獨(dú)立，都為零均值的高斯白噪聲。同時(shí)，噪聲驅(qū)動(dòng)矩陣G(k)是已知的。首先，EKF利用非線性函數(shù)的局部線性特診，將非線性模型局部線性化。由系統(tǒng)狀態(tài)方程式（1.5），將非線性函數(shù)f[*]圍繞濾波值處做一階Taylor展開,得： (1.7)令 (1.8) (1.9)則狀態(tài)方程為 (1.10)初始值為X(0)=E[X(0)]。同卡爾曼濾波基本方程相比，式（1.10）在已經(jīng)求得前一步濾波值的條件下，在狀態(tài)方程（5.3）基礎(chǔ)上增加了非隨機(jī)的為外作用項(xiàng)。由系統(tǒng)狀態(tài)方程式（1.6），將非線性函數(shù)h[*]利用泰勒展開式在本輪的狀態(tài)預(yù)測值處展開得： (1.11)令 (1.12) (1.13)則觀測方程為： (1.14)得到的結(jié)果與（1.6）相比，也是多了一個(gè)隨機(jī)的外部作用項(xiàng)y(k)。對線性化后的模型式（1.5）與式（1.6）應(yīng)用卡爾曼濾波基本方程，得擴(kuò)展卡爾曼濾波算法的遞推步驟如下：初始條件： (1.15)狀態(tài)變量預(yù)測估計(jì)： (1.16)誤差協(xié)方差預(yù)測估計(jì)： (1.17)卡爾曼增益計(jì)算： (1.18)狀態(tài)變量最優(yōu)估計(jì)： (1.19)誤差協(xié)方差測量更新： (1.20)式中，濾波初值和濾波協(xié)方差矩陣的初值分別為：隨著時(shí)間推移，重復(fù)公式(1.16)-(1.220)，可以得到每一時(shí)刻系統(tǒng)最優(yōu)狀態(tài)變量估計(jì)值與誤差協(xié)方差估計(jì)值。同線性卡爾曼濾波基本方程相比，狀態(tài)轉(zhuǎn)移和觀測矩陣由f[*]和h[*]的雅可比矩陣代替。假設(shè)狀態(tài)變量有n維，即，則相應(yīng)雅可比矩陣的求法如下：，1.2.2無跡卡爾曼上面提到的擴(kuò)展卡爾曼濾波算法簡單易操作，在工業(yè)中有廣泛的應(yīng)用。但是它也存在很多缺點(diǎn)：需要計(jì)算非線性模型的雅克比矩陣，計(jì)算大，易出錯(cuò)，難得到；忽略高階項(xiàng)，估計(jì)精度大受影響；模型不確定性的魯棒性很差；在系統(tǒng)達(dá)到平穩(wěn)狀態(tài)時(shí)，將喪失對突變狀態(tài)的跟蹤能力；如果系統(tǒng)的誤差傳播函數(shù)不能很好的用線性函數(shù)來逼近，可能會(huì)導(dǎo)致濾波器發(fā)散。無跡卡爾曼濾波是在卡爾曼濾波和變換的基礎(chǔ)上發(fā)展而來的，它是利用無損變換使線性假設(shè)下的卡爾曼濾波應(yīng)用于非線性系統(tǒng)。無跡卡爾曼濾波與擴(kuò)展卡爾曼濾波不同的是，它并不是線性化處理估計(jì)點(diǎn)附近的非線性方程f和h，而是由無跡變換確定的采樣點(diǎn)來表示非線性函數(shù)的高斯分布，進(jìn)而近似系統(tǒng)狀態(tài)的概率密度以達(dá)到狀態(tài)估計(jì)的目的REF_Ref25055\n\h[23]。無跡變換REF_Ref25454\n\h[24](UnscentedTransform,UT)方法與KF濾波算法是UKF濾波算法的理論基礎(chǔ)。UKF濾波算法和EKF濾波算法一樣都是遞歸式貝葉斯估計(jì)方法。但是UKF濾波算法進(jìn)行線性化用的是UT變換方法，這個(gè)方法可以代替EKF濾波算法設(shè)計(jì)中的線性化過程，UT變換方法是近似采樣點(diǎn)的后驗(yàn)概率密度。UKF濾波算法可分為以下三個(gè)部分：第一步：計(jì)算sigma點(diǎn)將非線性測量系統(tǒng)通過無跡變換產(chǎn)生sigma點(diǎn)。構(gòu)造相應(yīng)的權(quán)值，，...，進(jìn)而得到該非線性測量系統(tǒng)的統(tǒng)計(jì)特性。第二步：預(yù)測與線性kalman濾波一樣，UKF采用過程模型對下一時(shí)刻的狀態(tài)均值與協(xié)方差進(jìn)行預(yù)測。第三步：測量更新測量更新算法與經(jīng)典的KF濾波算法的更新方法一樣，都是通過不斷地預(yù)測狀態(tài)向量、誤差協(xié)方差矩陣，計(jì)算卡爾曼增益，修正狀態(tài)向量、誤差協(xié)方差矩陣，進(jìn)行更新。圖1.2-6無跡卡爾曼濾波算法框圖1.無跡變換（UT）的原理無跡變換就是針對EKF濾波算法的缺點(diǎn)提出的，UT的主要思想是“近似概率分布要比近似非線性函數(shù)更容易”。UT變換計(jì)算均值和協(xié)方差，通過含有均值和協(xié)方差的確定的點(diǎn)集（稱作sigmapoints）來近似概率分布，通過系統(tǒng)的非線性模型，產(chǎn)生繁衍的sigmapoint，經(jīng)過選擇合適的權(quán)值估計(jì)均值和協(xié)方差。避免了求解雅克比矩陣。這種方法把系統(tǒng)當(dāng)作“黑盒”來處理，因而不依賴于非線性的具體形式。UKF具有更高的估計(jì)精度，滿足了具有各種特殊要求的非線性濾波和控制方面的應(yīng)用。UT變換是用固定數(shù)量的參數(shù)去近似一個(gè)高斯分布，其實(shí)現(xiàn)原理為：在原先分布中按某一規(guī)則取一些點(diǎn)，使這些點(diǎn)的均值和協(xié)方差與原狀態(tài)分布的均值和協(xié)方差相等；將這些點(diǎn)代入非線性函數(shù)中，相應(yīng)得到非線性函數(shù)值點(diǎn)集，通過這些點(diǎn)集可求取變換的均值和協(xié)方差。對任何一種非線性系統(tǒng)，當(dāng)高斯型狀態(tài)微量經(jīng)由非線性系統(tǒng)進(jìn)行傳遞，進(jìn)而利用這組采樣點(diǎn)能獲取精確到三階矩的后驗(yàn)均值和協(xié)方差。UT變換的特點(diǎn)是對非線性函數(shù)的概率密度分布進(jìn)行近似，而不是對非線性函數(shù)進(jìn)行近似，即使系統(tǒng)模型復(fù)雜，也不增加算法實(shí)現(xiàn)的難度；而且所得到的非線性函數(shù)的統(tǒng)計(jì)量的準(zhǔn)確性可以達(dá)到三階；除此之外，它不需要計(jì)算雅可比矩陣，可以處理不可導(dǎo)非線性函數(shù)。設(shè)非線性變換f=f(x)，協(xié)方差P和均值x是已知的，狀態(tài)向量x為n維隨機(jī)向量。下面通過UT變換來計(jì)算y的統(tǒng)計(jì)特征，那么先求解2n+1個(gè)Sigma點(diǎn)X和權(quán)值ω。(1)求解2n+1個(gè)Sigma點(diǎn)，即采樣點(diǎn)。 (1.21)式中，(P)T(P(2)計(jì)算上述采樣點(diǎn)對應(yīng)的權(quán)值。 (1.22)式中，上標(biāo)表示第幾個(gè)采樣點(diǎn)；m為平均值；c為協(xié)方差；a為控制采樣點(diǎn)的狀態(tài);是比例參數(shù)，可以降低預(yù)測誤差，k為待選參數(shù)，它的取值不受界限的限制，并能保證尸為半正定矩陣;是權(quán)系數(shù)，它可以合并方程中的高階項(xiàng)進(jìn)而將高階項(xiàng)的影響考慮在內(nèi)。UT變換得到的Sigma點(diǎn)集具體有下述性質(zhì)：由于Sigma點(diǎn)集圍繞均值對稱分布并且對稱點(diǎn)具有相同的權(quán)值，因此Sigma集合的樣本均值為，與隨機(jī)向量X的均值相同。Sigma點(diǎn)集的樣本方差與隨機(jī)向量X的方差相同。任意正太分布的Sigma點(diǎn)集都是由標(biāo)準(zhǔn)正太分布的Sigma集合經(jīng)過一個(gè)變換得到的。2.UKF算法對于不同時(shí)刻k，非線性系統(tǒng)可由具有W(k)的隨機(jī)變量X和具有V(k)的觀測變量Z構(gòu)成并表示為: (1.23)式中，f是非線性狀態(tài)方程函數(shù)；h是非線性觀測方程函數(shù)。設(shè)W(k)含有協(xié)方差陣Q，V(k)含有協(xié)方差陣R，不同時(shí)刻k隨機(jī)變量X的UKF濾波步驟如下:(1)利用式(1.21)、式(1.22)取得一組采樣點(diǎn)及采樣點(diǎn)的權(quán)值： (1.24)(2)一步預(yù)測2n+1個(gè)Sigma點(diǎn)集的結(jié)果為： (1.25)計(jì)算非線性系統(tǒng)狀態(tài)一步預(yù)測的值及協(xié)方差陣。其中權(quán)值通過式(1.22)得到。這不同于普通卡爾曼濾波算法，普通卡爾曼濾波算法只需通過上一時(shí)刻的狀態(tài)代入狀態(tài)方程，計(jì)算一次就可以得到狀態(tài)的預(yù)測；而UKF是利用一組Sigma的預(yù)測值，并對它們進(jìn)行加權(quán)求平均值，進(jìn)而得到系統(tǒng)狀態(tài)量的一步預(yù)測： (1.26) (1.27)(4)根據(jù)式(3)中一步預(yù)測結(jié)果的值，進(jìn)行相同的變換得到新Sigma點(diǎn)集 (1.28)(5)將上一步驟的新Sigma點(diǎn)集代入式(1)中，得到觀測量 (1.29)(6)通過步驟(5)的sigma點(diǎn)集的觀測值計(jì)算得到觀測后的均值和協(xié)方差 (1.30) (1.30) (1.31)(7)計(jì)算卡爾曼增益 (1.32)(8)最后狀態(tài)更新和協(xié)方差更新的結(jié)果為 (1.33) (1.34)由此可以看出，無跡卡爾曼濾波在處理非線性濾波時(shí)，并不需要在估計(jì)點(diǎn)處作泰勒級數(shù)展開，然后進(jìn)行前n階近似，而是在估計(jì)點(diǎn)附近進(jìn)行UT變換使。使獲得的Sigma點(diǎn)集的均值和協(xié)方差與原統(tǒng)計(jì)特征匹配。再直接對這些Sigma點(diǎn)集進(jìn)行非線性映射，來近似狀態(tài)概率密度。這種近似其實(shí)既是一種統(tǒng)計(jì)近似而非方差的解。1.3回歸評估標(biāo)準(zhǔn)（1）均方誤差(meansquarederror,MSE)均方誤差是指參數(shù)估計(jì)值與參數(shù)真值之差平方的期望值，記為MSE。MSE是衡量平均誤差的一種較方便的方法，MSE可以評價(jià)數(shù)據(jù)的變化程度，均方誤差的公式如下： (1.35)其中yi與y誤差平方和(SumoftheSquaredErrors,SSE)誤差平方和又稱\t"/item/%E8%AF%AF%E5%B7%AE%E5%B9%B3%E6%96%B9%E5%92%8C/_blank"殘差平方和、\t"/item/%E8%AF%AF%E5%B7%AE%E5%B9%B3%E6%96%B9%E5%92%8C/_blank"組內(nèi)平方和等，根據(jù)n個(gè)觀察值擬合適當(dāng)?shù)哪Ｐ秃?，余下未能擬合部份(真實(shí)值-平均值)稱為殘差。 (1.36)其中，yi與yi分別為測試集上真實(shí)值與估計(jì)值，所有n個(gè)殘差平方之和稱誤差平方和。在\t"/item/%E8%AF%AF%E5%B7%AE%E5%B9%B3%E6%96%B9%E5%92%8C/_blank"回歸分析中通常用SSE表示，其大小用來表明函數(shù)擬合的好壞。將殘差平方和除以自由度n-p-1(其中p為自變量個(gè)數(shù))可以作為誤差方差σ2的\t"/item/%E8%AF%AF%E5%B7%AE%E5%B9%B3%E6%96%B9%E5%92%8C/_blank"無偏估計(jì)，通常用來檢驗(yàn)擬合的模型是否顯著。（3）平均絕對誤差(meanabsoluteerror,MAE) (1.37)均方根誤差為觀測值與真值值的誤差絕對值的平均值與MSE的區(qū)別在于MSE先對偏差做了一次平方，這樣，如果誤差的離散度高，即，如果最大偏差值大，MSE就放大了RMSE>MAE一組正數(shù)的平均數(shù)的平方小于每個(gè)數(shù)的平方和的平均數(shù)。均方根誤差（rootmeansquirederror，RMSE）均方根誤差亦稱標(biāo)準(zhǔn)誤差，是均方誤差的算術(shù)平方根。它是觀測值與真值偏差的平方和觀測次數(shù)n比值的平方根，在實(shí)際測量中，觀測次數(shù)n總是有限的，真值只能用最可信賴（最佳）值來代替。方根誤差對一組測量中的特大或特小