本發(fā)明涉及的是一種魯棒自適應(yīng)濾波和狀態(tài)估計方法，具體地說是一種基于huber估計的魯棒自適應(yīng)濾波和狀態(tài)估計方法。

背景技術(shù)：

無跡信息濾波器(uif)是一種新興的濾波和狀態(tài)估計方法，本質(zhì)上是一種通過協(xié)方差矩陣的逆來更新的kalman濾波器，鑒于無跡信息濾波對協(xié)方差矩陣初值無要求、有比普通kalman濾波器更易于計算的更新方程及易于多源融合的擴(kuò)展等優(yōu)點得到廣泛關(guān)注。然而對于無人艇不確定運動模型、海洋干擾模型和不確定傳感器量測噪聲同時存在的雙重不確定系統(tǒng)時，常規(guī)無跡信息濾波器的濾波精度會明顯下降甚至無法收斂。而且現(xiàn)有的海浪濾波和狀態(tài)估計技術(shù)大多針對某一不確定性問題進(jìn)行研究并提出了較好的解決方法，對于上述包含兩種及以上不確定因素的雙重不確定系統(tǒng)還未給出有效的濾波和狀態(tài)估計方法。

雙重不確定系統(tǒng)的濾波和狀態(tài)估計問題廣泛存在于運載器(水面無人艇、水下航行器等)控制領(lǐng)域，備受許多學(xué)者關(guān)注。

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

本發(fā)明的目的在于提供一種能夠保證雙重不確定系統(tǒng)濾波和狀態(tài)估計的精度和穩(wěn)定性的基于huber估計的魯棒自適應(yīng)濾波和狀態(tài)估計方法。

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

首先通過在線估計一步預(yù)測雙重不確定性系統(tǒng)的協(xié)方差矩陣的上界來計算估計值，只對時間更新過程進(jìn)行修改，同時采用線性矩陣不等式的方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，得到應(yīng)對不確定性模型的自適應(yīng)無跡信息濾波器；然后應(yīng)用矩陣求逆定理并采用huber估計的方法對量測更新過程進(jìn)行修改，得到最終的估計值，具體包括如下步驟：

(1)、初始化，初始狀態(tài)估計值設(shè)為0，初始協(xié)方差矩陣選擇某一常值矩陣，不能選擇零矩陣，滿足

(2)、時間更新，按照常規(guī)無跡信息濾波的時間更新過程計算和表示估計狀態(tài)變量的均值，pk|k-1表示一步預(yù)測協(xié)方差；

(3)、自適應(yīng)時間更新，計算協(xié)方差上界估計值后，用代替pk|k-1，結(jié)合無跡信息濾波的量測更新方程，求得最終的估計值及其協(xié)方差矩陣采用線性矩陣不等式方法進(jìn)行在線自適應(yīng)參數(shù)調(diào)整；

(4)、基于huber估計的量測更新，應(yīng)用矩陣求逆定理并在量測更新部分引入huber估計方法，更新過程中的pk|k-1均替換為第(3)步的且中間變量也相應(yīng)地增加上標(biāo)(u)，最終得到估計結(jié)果和

本發(fā)明還可以包括：

1、所述的雙重不確定系統(tǒng)的不確定性集成在一起表示為：

其中，為無人艇狀態(tài)變量，u為推進(jìn)器推力和環(huán)境作用力，為確定噪聲，e為適維常數(shù)矩陣，h為適維常數(shù)矩陣，非線性函數(shù)f(·)包含未知建模誤差、時變參數(shù)不確定性因素，v為適維量測噪聲、指電羅經(jīng)和gps的傳感器測量噪聲；

雙重不確定系統(tǒng)的非線性離散模型為：

xk＝fa(xk-1,uk-1)+δ+ek-1wk-1

yk＝hkxk+vk，

其中，xk∈rⁿ和yk∈r^m分別為狀態(tài)變量和量測變量，函數(shù)fa(·)表示標(biāo)稱模型，為模型中的確定部分；δ表示建模誤差，且有界，設(shè)||δ||≤χ,χ≥0；wk和vk為相互獨立高斯白噪聲，方差分別為qk和rk，維數(shù)分別為nv和nn；k為采樣數(shù)、為非負(fù)整數(shù)。

2、所述按照常規(guī)無跡信息濾波的時間更新過程計算和pk|k-1具體包括：

設(shè)置初值使得的初值滿足p0^u≥p0，那么和都滿足以上不等式，根據(jù)來得到當(dāng)前時刻的和

經(jīng)過一系列不等式縮放，得一步預(yù)測協(xié)方差的保守上界，按照其結(jié)構(gòu)進(jìn)行參數(shù)化得

其中，pk|k-1為一步預(yù)測值協(xié)方差，λk,α,β為待設(shè)計的濾波器參數(shù)，tr(·)表示矩陣的跡，i表示適維單位陣；

首先通過調(diào)節(jié)參數(shù)α,β對建模誤差進(jìn)行補(bǔ)償，其次采用線性矩陣不等式方法調(diào)整自適應(yīng)無跡信息濾波參數(shù)λk；

由一步預(yù)測值的協(xié)方差得新息協(xié)方差矩陣的預(yù)測值為：

同時，通過引入一個加權(quán)因子μ∈[0,1]，得新息協(xié)方差矩陣觀測值的無偏估計為

其中，為新息方程，且μ的大小會影響濾波器對噪聲特性和模型不確定性的敏感性，當(dāng)μ＝0時，僅由當(dāng)前時刻的觀測值決定，當(dāng)μ＝1時，還會受到歷史觀測值影響；

通過如下最優(yōu)問題來調(diào)節(jié)λk，同時參數(shù)更新時間間隔通過下式的計算間隔確定：

其中，參數(shù)λk在線調(diào)節(jié)，參數(shù)α,β離線調(diào)節(jié)；

當(dāng)計算出后，用其代替pk|k-1，然后采用huber估計方法的量測更新方程求得最終的估計值及其協(xié)方差矩陣

3、huber估計的魯棒無跡信息濾波器是采用如下方法設(shè)計的：

初始化及時間更新部分與常規(guī)無跡信息濾波和狀態(tài)估計算法一致，首先k時刻的估計值表示為

其中δk表示一步預(yù)測誤差，協(xié)方差為pk|k-1；

根據(jù)常規(guī)無跡信息濾波和狀態(tài)估計算法的推導(dǎo)過程及結(jié)果，存在如下映射

其中選取線性的量測方程，

線性回歸模型如下：

定義

則

其中，為殘差，

令

其中，表示常規(guī)無跡信息濾波量測更新后的初始估計值；

huber估計是通過最小化目標(biāo)函數(shù)j來估計被估量，則有如下目標(biāo)函數(shù)

其中，ri表示殘差，σi是比例因子為單位陣，殘差是被估量的函數(shù)，

為使目標(biāo)函數(shù)最小，則

其中，huber估計的ρ函數(shù)、核函數(shù)ψ(r)和權(quán)值函數(shù)q(r)分別表示如下，γ為調(diào)節(jié)參數(shù)，

則最小目標(biāo)函數(shù)為：

其中，

通過迭代法求最小目標(biāo)函數(shù)，迭代公式為：

其中，j為迭代次數(shù)、取j＝1，初值取或者取常規(guī)無跡信息濾波量測更新后的估計值，

然后重新定義信息濾波中的信息矩陣和信息向量：

并修改協(xié)方差矩陣pk|k-1的計算公式為：

最后按照信息濾波的形式進(jìn)行量測更新：

其中的信息狀態(tài)增量及其關(guān)聯(lián)矩陣，為：

本發(fā)明針對雙重不確定性無人艇的濾波和狀態(tài)估計問題，提出了一種基于huber估計的魯棒自適應(yīng)濾波和狀態(tài)估計方法，保證雙重不確定系統(tǒng)濾波和狀態(tài)估計的精度和穩(wěn)定性。主要包括：

一、雙重不確定系統(tǒng)

無人艇模型參數(shù)會隨著船速、水深和負(fù)載等因素的變化而變化，因此無人艇數(shù)學(xué)模型存在不確定性；同時不管無人艇處于何種運動狀態(tài)，均要受到高低頻海浪干擾，其建模過程存在未知建模誤差、時變參數(shù)等不確定性因素；對于不確定量測噪聲，當(dāng)傳感器測量數(shù)據(jù)沒有受到干擾時，可以采用高斯白噪聲近似模擬，但實際測量過程中，測量數(shù)據(jù)由于受到各種干擾的污染，使得量測噪聲為非高斯分布。因此系統(tǒng)進(jìn)行濾波和狀態(tài)估計時實際存在兩種及以上不確定因素，即雙重不確定系統(tǒng)。

二、自適應(yīng)無跡信息濾波器

對于不確定性模型的建模誤差，引入估計值狀態(tài)協(xié)方差的上界，只對時間更新過程進(jìn)行修改，同時采用線性矩陣不等式的方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，進(jìn)而推導(dǎo)出應(yīng)對模型不確定性的自適應(yīng)無跡信息濾波和狀態(tài)估計方法。

三、利用huber估計的魯棒無跡信息濾波器

對于不確定量測噪聲，應(yīng)用矩陣求逆定理并采用huber估計的方法對量測更新過程進(jìn)行修改，即保留了信息濾波器量測更新的結(jié)構(gòu)又解決了量測噪聲非高斯分布時的濾波和狀態(tài)估計問題。

本發(fā)明包括以下有益效果：

1、本發(fā)明的無跡信息濾波器對協(xié)方差矩陣初值無要求、易于多源融合的擴(kuò)展。

2、本發(fā)明對建模誤差有魯棒性，通過在線估計一步預(yù)測協(xié)方差矩陣的上界來計算估計值，并且只修改時間更新過程。

3、本發(fā)明應(yīng)用矩陣求逆定理并引入huber估計方法，只對量測更新過程進(jìn)行修改，不會在huber估計中引入線性化誤差，不會改變原有無跡信息濾波器的結(jié)構(gòu)。

4、本發(fā)明所述的基于huber估計的無跡信息濾波和狀態(tài)估計方法，可以保證雙重不確定無人艇的濾波精度和穩(wěn)定性。

本發(fā)明涉及的是一種基于huber估計的魯棒自適應(yīng)無跡信息濾波和狀態(tài)估計(huber-basedadaptiveunscentedinformationfilter，即hauif)技術(shù)，應(yīng)用于欠驅(qū)動水面無人艇，即具有雙重不確定性無人艇的濾波和狀態(tài)估計?；趆uber估計和無跡信息濾波是一種處理具有雙重不確定系統(tǒng)的濾波和狀態(tài)估計的有效方法。這種方法通過在線估計一步預(yù)測協(xié)方差矩陣的上界來計算估計值，只對時間更新過程進(jìn)行修改，同時采用線性矩陣不等式的方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，推導(dǎo)出應(yīng)對不確定性模型的自適應(yīng)無跡信息濾波器(auif)；然后應(yīng)用矩陣求逆定理并采用huber估計的方法對量測更新過程進(jìn)行修改，得到最終的估計值，即保留了信息濾波器量測更新的結(jié)構(gòu)又提高了量測噪聲非高斯分布時的濾波精度。

附圖說明

圖1為本發(fā)明所述濾波和狀態(tài)估計方法的結(jié)構(gòu)框圖；

圖2為無人艇的局部坐標(biāo)系與船體坐標(biāo)系示意圖；

圖3為雙重不確定系統(tǒng)的常規(guī)無跡信息濾波器(uif)估計誤差；

圖4為雙重不確定系統(tǒng)的自適應(yīng)無跡信息濾波器(auif)估計誤差；

圖5為雙重不確定系統(tǒng)的hauif估計誤差。

具體實施方式

為使本發(fā)明的上述目的、特征和優(yōu)點能夠更加明顯易懂，下面結(jié)合圖1至圖5舉例對本發(fā)明作進(jìn)一步詳細(xì)的說明。

結(jié)合圖1，本發(fā)明所述濾波和狀態(tài)估計方法主要包括如下步驟：

1、初始化，初始狀態(tài)估計值設(shè)為0，初始協(xié)方差矩陣根據(jù)情況選擇某一常值矩陣，不能選擇零矩陣，因為需要滿足

2、時間更新，按照常規(guī)無跡信息濾波的時間更新過程計算和pk|k-1；

3、自適應(yīng)時間更新，計算協(xié)方差上界估計值后，用其代替pk|k-1，結(jié)合無跡信息濾波的量測更新方程即可求得最終的估計值及其協(xié)方差矩陣采用線性矩陣不等式方法進(jìn)行在線自適應(yīng)參數(shù)調(diào)整；

4、基于huber估計的量測更新，應(yīng)用矩陣求逆定理并在量測更新部分引入huber估計方法，注意更新過程中的pk|k-1均需要替換為第3步的且中間變量也需相應(yīng)地增加上標(biāo)(u)，最終得到估計結(jié)果和

本實施方式所述的基于huber估計的魯棒自適應(yīng)濾波和狀態(tài)估計方法的特點包括：

一、雙重不確定系統(tǒng)

無人艇模型參數(shù)會隨著船速、水深和負(fù)載等因素的變化而變化，因此無人艇數(shù)學(xué)模型存在不確定性；同時不管無人艇處于何種運動狀態(tài)，均要受到高低頻海浪干擾，其建模過程存在未知建模誤差、時變參數(shù)等不確定性因素；對于不確定量測噪聲，當(dāng)傳感器測量數(shù)據(jù)沒有受到干擾時，可以采用高斯白噪聲近似模擬，但實際測量過程中，測量數(shù)據(jù)由于受到各種干擾的污染，使得量測噪聲為非高斯分布。因此系統(tǒng)進(jìn)行濾波和狀態(tài)估計時實際存在兩種及以上不確定因素，即雙重不確定系統(tǒng)。

二、自適應(yīng)無跡信息濾波器

對于不確定性模型的建模誤差，引入估計值狀態(tài)協(xié)方差的上界，只對時間更新過程進(jìn)行修改，同時采用線性矩陣不等式的方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，進(jìn)而推導(dǎo)出應(yīng)對模型不確定性的自適應(yīng)無跡信息濾波和狀態(tài)估計方法。

三、利用huber估計的魯棒無跡信息濾波器

對于不確定量測噪聲，應(yīng)用矩陣求逆定理并采用huber估計的方法對量測更新過程進(jìn)行修改，即保留了信息濾波器量測更新的結(jié)構(gòu)又解決了量測噪聲非高斯分布時的濾波和狀態(tài)估計問題。

結(jié)合圖2所述的雙重不確定系統(tǒng)如下：

1、不確定無人艇數(shù)學(xué)模型：

通過航向自動舵及gps設(shè)置的航跡點來間接控制無人艇的航跡，無人艇航向模型可寫為：

其中，ψ為艏向角，r為艏搖角速度，為其一階導(dǎo)數(shù)，δ為實際舵角，hn(r)為描述操縱特性的非線性函數(shù)，b表示舵角偏移量，其作用是為了抵消慢變干擾，w1∈r表示系統(tǒng)噪聲，t,k為待設(shè)計參數(shù)。

為方便描述，可用一個統(tǒng)一的形式表示為：

其中，函數(shù)f1(·)為非線性函數(shù)，x0＝[ψ,r]^t,u＝δ,wp＝w1。

2、不確定海洋干擾模型：

高頻海浪干擾為一階波浪力，可使用二階濾波器對其進(jìn)行建模，狀態(tài)空間形式為：

ηw＝cwξ

其中，ξ∈r為高頻運動狀態(tài)，ηw∈r為高頻海浪引起的無人艇艏向角變化，ww為零均值白噪聲過程，aw,ew,cw為適維矩陣。

低頻海浪干擾為二階波浪力、海風(fēng)和海流等，可采用在輸入中疊加一個慢變信號的方式來模擬，慢變干擾b采用一階馬爾科夫過程來模擬：

其中，tb為適維時間常數(shù)，一般取值較大，wb為適維高斯白噪聲。

不確定性集成在一起可表示為：

y＝hx+v

其中，為無人艇狀態(tài)變量，u為推進(jìn)器推力和環(huán)境作用力，為確定噪聲，e為適維常數(shù)矩陣，h為適維常數(shù)矩陣，非線性函數(shù)f(·)包含未知建模誤差、時變參數(shù)等不確定性因素，v為適維量測噪聲，主要指電羅經(jīng)和gps的傳感器測量噪聲。

綜上雙重不確定無人艇非線性離散模型為：

xk＝fa(xk-1,uk-1)+δ+ek-1wk-1

yk＝hkxk+vk

其中，xk∈rⁿ和yk∈r^m分別為狀態(tài)變量和量測變量，函數(shù)fa(·)表示標(biāo)稱模型，為模型中的確定部分；δ表示建模誤差，且有界，假設(shè)||δ||≤χ,χ≥0；wk和vk為相互獨立高斯白噪聲，方差分別為qk和rk，維數(shù)分別為nv和nn；k為采樣數(shù)(非負(fù)整數(shù))，l＝n+nv。

結(jié)合圖1圖3，所述的常規(guī)無跡信息濾波和狀態(tài)估計算法為：

1、初始化：

式中，表示估計狀態(tài)變量的初值，表示全維狀態(tài)變量的初值，表示全維協(xié)方差矩陣的初值。

2、計算系統(tǒng)狀態(tài)sigma點：

其中，λ＝α²n-n為比例系數(shù)，α∈[0,1]通常取一個很小的數(shù)。

3、時間更新：

其中，表示估計狀態(tài)變量的均值，pk|k-1表示一步預(yù)測協(xié)方差，χi為狀態(tài)變量對應(yīng)的sigma點，wi為sigma點的權(quán)值計算如下

4、定義中間變量(信息矩陣和信息向量)：

5、量測更新：

式中，pxy表示協(xié)方差矩陣。

結(jié)合圖1圖4，步驟二中自適應(yīng)無跡信息濾波器的具體設(shè)計過程為：

對不確定模型的建模誤差，引入在線估計一步預(yù)測協(xié)方差矩陣的上界，同時采用線性矩陣不等式方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，使得估計結(jié)果對模型不確定性具有魯棒自適應(yīng)性。記為估計值協(xié)方差上界，為一步預(yù)測值的協(xié)方差上界，則有：

設(shè)置足夠大的初值使得的初值滿足那么和都將滿足以上不等式，根據(jù)來得到當(dāng)前時刻的和

經(jīng)過一系列的不等式縮放，可得一步預(yù)測協(xié)方差的較為保守的上界，按照其結(jié)構(gòu)進(jìn)行參數(shù)化可得

其中，pk|k-1為一步預(yù)測值協(xié)方差，λk,α,β為待設(shè)計的濾波器參數(shù)，tr(·)表示矩陣的跡，i表示適維單位陣。

分析上式可知：第一項λkpk|k-1表示計算過程盡可能大地利用一步預(yù)測協(xié)方差矩陣pk|k-1，而pk|k-1可以通過時間更新過程得到，這一項可以應(yīng)對建模誤差對時間更新過程的影響；第二項與狀態(tài)估計結(jié)果的協(xié)方差上界有關(guān)，可以應(yīng)對建模誤差對最終估計結(jié)果(量測更新之后)的影響；第三項是一個常值，可以應(yīng)對模型中的常值不確定性。對于非線性離散系統(tǒng)模型的建模誤差項假設(shè)滿足||δ||≤χ,χ≥0，是無需知道其上界具體值的，并且其在實際中也較難獲得。首先通過調(diào)節(jié)參數(shù)α,β即可對建模誤差進(jìn)行較好的補(bǔ)償，其次采用線性矩陣不等式方法調(diào)整自適應(yīng)無跡信息濾波參數(shù)λk：

由一步預(yù)測值的協(xié)方差上界知信息協(xié)方差矩陣的預(yù)測值要大于其觀測值。由一步預(yù)測值的協(xié)方差可得，新息協(xié)方差矩陣的預(yù)測值為：

同時，通過引入一個加權(quán)因子μ∈[0,1]，可得新息協(xié)方差矩陣觀測值的無偏估計為

其中，為新息方程，且μ的大小會影響濾波器對噪聲特性和模型不確定性的敏感性，當(dāng)μ＝0時，僅由當(dāng)前時刻的觀測值決定，當(dāng)μ＝1時，還會受到歷史觀測值影響。

為使得減小估計的保守性，可通過如下最優(yōu)問題來調(diào)節(jié)λk，同時參數(shù)更新時間間隔可以通過下式的計算間隔確定：

其中，參數(shù)λk在線調(diào)節(jié)，參數(shù)α,β離線調(diào)節(jié)。

由于為一步預(yù)測值的協(xié)方差上界不等式成立的必要條件而不是充分條件，所以α,β對濾波器的性能非常重要。當(dāng)計算出后，用其代替pk|k-1，然后采用huber估計方法的量測更新方程即可求得最終的估計值及其協(xié)方差矩陣

結(jié)合圖1圖5，利用huber估計的魯棒無跡信息濾波器的具體設(shè)計過程為：

對于不確定量測噪聲，應(yīng)用矩陣求逆定理并在量測更新部分引入huber估計方法，保留了無跡信息濾波量測更新的結(jié)構(gòu)。

初始化及時間更新部分與常規(guī)無跡信息濾波和狀態(tài)估計算法一致，首先k時刻的估計值可表示為

其中δk表示一步預(yù)測誤差，協(xié)方差為pk|k-1。

根據(jù)常規(guī)無跡信息濾波和狀態(tài)估計算法的推導(dǎo)過程及結(jié)果，存在如下映射

其中選取線性的量測方程。

線性回歸模型如下：

定義

則有

其中，為殘差。

令

其中，表示常規(guī)無跡信息濾波量測更新后的初始估計值。

huber估計的思想是通過最小化目標(biāo)函數(shù)j來估計被估量。則有如下目標(biāo)函數(shù)

其中，ri表示殘差，σi是比例因子為單位陣，殘差一般是被估量的函數(shù)。

為使目標(biāo)函數(shù)最小，則需

其中，huber估計的ρ函數(shù)、核函數(shù)ψ(r)和權(quán)值函數(shù)q(r)分別表示如下，γ為調(diào)節(jié)參數(shù)，表達(dá)式如下

則最小目標(biāo)函數(shù)為：

其中，

通過迭代法求最小目標(biāo)函數(shù)，迭代公式為：

其中，j為迭代次數(shù)，一般取j＝1即可，初值取或者取常規(guī)無跡信息濾波量測更新后的估計值。

然后重新定義信息濾波中的信息矩陣和信息向量：

并修改協(xié)方差矩陣pk|k-1的計算公式為：

最后則可按照信息濾波的形式進(jìn)行量測更新：

但需修改其中的信息狀態(tài)增量及其關(guān)聯(lián)矩陣，為：

本發(fā)明適用于包括不確定無人艇模型、海洋干擾模型和不確定傳感器量測噪聲同時存在的雙重不確定無人艇的濾波和狀態(tài)估計。提出一種基于huber估計的魯棒自適應(yīng)濾波和狀態(tài)估計方法：這種方法通過在線估計一步預(yù)測協(xié)方差矩陣的上界來計算估計值，只對時間更新過程進(jìn)行修改，同時采用線性矩陣不等式的方法進(jìn)行自適應(yīng)參數(shù)調(diào)整，推導(dǎo)出應(yīng)對不確定模型的自適應(yīng)無跡信息濾波器(auif)；然后應(yīng)用矩陣求逆定理并采用huber估計的方法對量測更新過程進(jìn)行修改，解決了量測噪聲非高斯分布時的濾波和狀態(tài)估計問題。通過附圖3-5比較uif、auif和hauif的估計誤差效果圖，可知hauif的濾波和狀態(tài)估計的精度高，穩(wěn)定性好。最終本發(fā)明所提出的魯棒自適應(yīng)濾波和狀態(tài)估計方法解決了雙重不確定無人艇的濾波和狀態(tài)估計問題，即保留了信息濾波器結(jié)構(gòu)的優(yōu)點有提高了濾波和狀態(tài)估計的精度和穩(wěn)定性，計算簡單應(yīng)用方便。