本發(fā)明涉及一種用于無人機(jī)的基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼的慣性/視覺/超聲波組合導(dǎo)航方法及裝置。

背景技術(shù)：

隨著導(dǎo)航系統(tǒng)微型化、智能化、自主化、低成本的發(fā)展趨勢(shì)，并且在導(dǎo)航系統(tǒng)自主化重要性日益突出的背景下，慣性/視覺組合導(dǎo)航技術(shù)以其良好的優(yōu)勢(shì)互補(bǔ)和自主特性，已經(jīng)成為了導(dǎo)航領(lǐng)域一個(gè)重要發(fā)展方向。在無人機(jī)的定點(diǎn)懸停、自主飛行與降落與機(jī)器人的目標(biāo)識(shí)別與跟蹤及環(huán)境檢測(cè)等任務(wù)中，慣性/視覺導(dǎo)航技術(shù)表現(xiàn)出能與外界環(huán)境相交互、動(dòng)態(tài)性能好、實(shí)時(shí)性高等諸多優(yōu)越性。在民用級(jí)慣性/視覺導(dǎo)航系統(tǒng)中，由于器件或設(shè)備本身精度和穩(wěn)定性的不足，系統(tǒng)參數(shù)和噪聲統(tǒng)計(jì)特性往往會(huì)隨著溫度、光照條件等外界環(huán)境的變化而發(fā)生顯著改變。對(duì)于實(shí)際的工程條件中，系統(tǒng)模型往往是非線性的、噪聲統(tǒng)計(jì)特性也因某些條件會(huì)在一定區(qū)間范圍內(nèi)波動(dòng)，比如在溫度變化、運(yùn)載體頻繁的姿態(tài)變化、光照條件不理想，或者視覺傳感器定焦困難等情形下，慣性傳感器和視覺傳感器的測(cè)量值往往會(huì)包含成分較為復(fù)雜的噪聲分量甚至?xí)霈F(xiàn)隨機(jī)野值，并且噪聲的成分通常也會(huì)隨著外界條件的變化而發(fā)生改變。傳統(tǒng)的卡爾曼濾波器在上述條件下將無法獲得系統(tǒng)的最優(yōu)估計(jì)，甚至?xí)l(fā)生濾波器發(fā)散而導(dǎo)致導(dǎo)航系統(tǒng)崩潰的現(xiàn)象。

研究者們一般在處理卡爾曼濾波過程中系統(tǒng)模型非線性的問題時(shí)，往往是將非線性系統(tǒng)通過近似方法線性化的方式得到系統(tǒng)的線性方程，這類方法主要包括擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波等。擴(kuò)展卡爾曼濾波算法在系統(tǒng)函數(shù)特征點(diǎn)附近通過泰勒展開的方式省略了系統(tǒng)模型高階項(xiàng)從而得到系統(tǒng)的線性模型，有效提高了濾波器的魯棒性和濾波精度。但當(dāng)系統(tǒng)參數(shù)或噪聲統(tǒng)計(jì)特性時(shí)變或者不確切已知時(shí)，擴(kuò)展卡爾曼濾波對(duì)此無能為力。區(qū)間卡爾曼濾波算法是在區(qū)間概念的基礎(chǔ)上，通過將系統(tǒng)或噪聲參數(shù)的不確定性建模為區(qū)間模型的一種處理無法精確對(duì)系統(tǒng)或噪聲建模時(shí)的魯棒濾波算法。區(qū)間卡爾曼濾波算法在不增加額外計(jì)算量的基礎(chǔ)上可有效降低模型不精確條件下對(duì)濾波器造成的影響。

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

本發(fā)明的技術(shù)解決問題是：對(duì)卡爾曼濾波技術(shù)進(jìn)行拓展和融合，結(jié)合擴(kuò)展卡爾曼方法和區(qū)間卡爾曼方法兩種濾波算法的優(yōu)點(diǎn)，建立了基于慣性/視覺/超聲波的新型組合導(dǎo)航方法。該方法有效克服了傳統(tǒng)方法由于非線性系統(tǒng)模型以及不確定因素影響下導(dǎo)航精度及魯棒性的欠缺，得到置信度更高、信息更為完備的導(dǎo)航參數(shù)，有效提高了導(dǎo)航系統(tǒng)在未知環(huán)境中的穩(wěn)定性和適應(yīng)性，增強(qiáng)了導(dǎo)航系統(tǒng)的實(shí)時(shí)性，可使運(yùn)載體完成過程更復(fù)雜、環(huán)境更惡劣的導(dǎo)航任務(wù)。

本發(fā)明為實(shí)現(xiàn)上述目的，采用如下技術(shù)方案：

一種基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼的無人機(jī)組合導(dǎo)航方法，包括：

通過慣性導(dǎo)航獲得無人機(jī)在導(dǎo)航坐標(biāo)系下的速度信息和位置信息，作為組合導(dǎo)航參數(shù)的狀態(tài)信息；通過視覺導(dǎo)航獲得無人機(jī)在攝像頭坐標(biāo)系下的沿水平方向上的光流速度信息和飛行高度信息，作為組合導(dǎo)航參數(shù)的觀測(cè)信息；

通過擴(kuò)展卡爾曼濾波將非線性系統(tǒng)線性化，并利用自適應(yīng)區(qū)間卡爾曼濾波將時(shí)變的系統(tǒng)參數(shù)建模為自適應(yīng)的區(qū)間模型，構(gòu)建組合導(dǎo)航濾波模型，融合慣性、視覺導(dǎo)航參數(shù)，對(duì)系統(tǒng)導(dǎo)航參數(shù)做出最優(yōu)估計(jì)。

作為優(yōu)選，所述組合導(dǎo)航濾波模型為：

其中，與分別為k時(shí)刻系統(tǒng)狀態(tài)向量的最優(yōu)估計(jì)和先驗(yàn)估計(jì)，為k-1時(shí)刻系統(tǒng)狀態(tài)向量的最優(yōu)估計(jì)；kk為k時(shí)刻卡爾曼濾波增益；與pk分別為k時(shí)刻一步預(yù)測(cè)誤差方差矩陣和估計(jì)誤差方差矩陣，pk-1則為k-1時(shí)刻的估計(jì)誤差方差矩陣；f代表系統(tǒng)狀態(tài)函數(shù)；bk為系統(tǒng)的控制分配矩陣；uk為系統(tǒng)的控制向量；ak為狀態(tài)方程的雅克比矩陣；hk為觀測(cè)方程的雅克比矩陣；wk為狀態(tài)噪聲分配矩陣；vk為觀測(cè)噪聲分配矩陣；zk為觀測(cè)向量；與分別為狀態(tài)噪聲矩陣和觀測(cè)噪聲矩陣的區(qū)間模型；上標(biāo)i為區(qū)間矩陣標(biāo)識(shí)，表示該矩陣向量為區(qū)間矩陣向量；表示觀測(cè)噪聲為零時(shí)，關(guān)于先驗(yàn)估計(jì)的系統(tǒng)觀測(cè)函數(shù)。

作為優(yōu)選，與分別表示為：

其中，δqk與δrk分別為狀態(tài)噪聲矩陣和觀測(cè)噪聲矩陣的變化范圍；

通過計(jì)算殘差的實(shí)際量測(cè)方差與理論方差的矩陣的跡之比來對(duì)濾波模型的準(zhǔn)確性作出評(píng)價(jià)；當(dāng)模型估計(jì)偏差較大時(shí)，根據(jù)δrk＝(τe^|ε-1|-1)·c修正觀測(cè)噪聲，其中，ak為平滑因子,τ是自適應(yīng)函數(shù)的比例因子,c為動(dòng)態(tài)觀測(cè)噪聲的基準(zhǔn)矩陣。

作為優(yōu)選，系統(tǒng)狀態(tài)向量和觀測(cè)向量分別為：

zk＝[vxvyzc]^t

其中，xn,yn,zn分別表示通過慣性導(dǎo)航得到的無人機(jī)在導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的位置，分別表示通過慣性導(dǎo)航得到的無人機(jī)在導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的速度；vx,vy分別表示通過視覺導(dǎo)航得到的無人機(jī)在攝像頭坐標(biāo)系下沿x,y兩個(gè)軸即水平方向上的光流速度信息，zc表示無人機(jī)的飛行高度。

作為優(yōu)選，狀態(tài)方程和觀測(cè)方程的雅克比矩陣分別為

其中，dt為狀態(tài)更新周期；l為視覺導(dǎo)航中視覺傳感器的焦距。

一種采用上述組合導(dǎo)航方法的基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼的無人機(jī)組合導(dǎo)航裝置，包括微機(jī)電系統(tǒng)慣性測(cè)量單元、超聲波傳感器、視覺傳感器和數(shù)字信號(hào)處理模塊；所述微機(jī)電系統(tǒng)慣性測(cè)量單元用于獲取載體的角速度和線速度信息；所述視覺傳感器用于獲取載體周圍圖像信息；所述超聲波傳感器用于獲取無人機(jī)飛行高度信息；所述數(shù)字信號(hào)處理模塊，包括：慣性導(dǎo)航解算單元，用于根據(jù)載體角速度和線速度信息解算得到無人機(jī)的姿態(tài)、速度和位置信息；視覺導(dǎo)航解算單元，用于根據(jù)載體周圍圖像信息及飛行高度解算得到無人機(jī)在水平方向上的速度信息；以及濾波算法單元，用于根據(jù)所獲取的導(dǎo)航參數(shù)結(jié)合組合導(dǎo)航濾波模型執(zhí)行擴(kuò)展自適應(yīng)區(qū)間卡爾曼濾波算法對(duì)系統(tǒng)導(dǎo)航參數(shù)做出最優(yōu)估計(jì)。

作為優(yōu)選,所述微機(jī)電系統(tǒng)慣性測(cè)量單元采用型號(hào)為mpu9250的傳感器，其內(nèi)包括三軸陀螺，三軸加速度計(jì)，三軸磁力計(jì)；所述數(shù)字信號(hào)處理模塊采用型號(hào)為tms320c5509的dsp；所述視覺傳感器型號(hào)為mt9v034；所述超聲波傳感器型號(hào)為mb1043。

本發(fā)明提出的一種基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼濾波的無人機(jī)的慣性/視覺/超聲波組合導(dǎo)航方法及裝置，其中新型的濾波方法融合了擴(kuò)展卡爾曼濾波和區(qū)間卡爾曼濾波兩者的優(yōu)點(diǎn)，主要解決了兩個(gè)問題，其一是將非線性系統(tǒng)模型通過擴(kuò)展卡爾曼方法得到了系統(tǒng)的線性模型；其二是通過區(qū)間概念建立狀態(tài)噪聲和觀測(cè)噪聲的區(qū)間模型，并通過自適應(yīng)方法動(dòng)態(tài)調(diào)整區(qū)間的變化范圍，以降低噪聲統(tǒng)計(jì)特性變化對(duì)濾波效果的影響。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：

(1)應(yīng)用于無人機(jī)的基于mems的慣性測(cè)量單元mpu9250、視覺傳感器mt9v034、超聲波傳感器mb1043、數(shù)字信號(hào)處理模塊tms320c5509的組合導(dǎo)航裝置易于實(shí)現(xiàn)，成本低廉，質(zhì)量較小，功耗較低，適合民用領(lǐng)域的推廣；

(2)融合了擴(kuò)展卡爾曼和區(qū)間卡爾曼兩種濾波算法的優(yōu)點(diǎn)，不僅可以對(duì)非線性系統(tǒng)進(jìn)行濾波降噪，還可以將系統(tǒng)參數(shù)的不確定性通過區(qū)間建模的方法進(jìn)行處理；

(3)避免了針對(duì)特殊環(huán)境優(yōu)化的復(fù)雜建模過程，將系統(tǒng)的噪聲統(tǒng)計(jì)特性描述為在一定范圍內(nèi)波動(dòng)的區(qū)間模型，并通過自適應(yīng)的方法動(dòng)態(tài)改變區(qū)間的范圍，是一種動(dòng)態(tài)的濾波算法，避免各種不確定性或隨機(jī)野值對(duì)算法的影響，可有效提高導(dǎo)航系統(tǒng)對(duì)不確定因素的適應(yīng)性，從而在溫度變化、運(yùn)載體頻繁的姿態(tài)變化、光照條件不理想，或者視覺傳感器無法定焦等情形下提供精度更高、數(shù)據(jù)更為可靠的導(dǎo)航定位服務(wù)。

附圖說明

圖1為本發(fā)明的用于無人機(jī)的組合導(dǎo)航系統(tǒng)原理示意圖；

圖2為本發(fā)明的視覺導(dǎo)航參數(shù)計(jì)算模型示意圖。

具體實(shí)施方式

下面結(jié)合具體實(shí)施例，進(jìn)一步闡明本發(fā)明，應(yīng)理解這些實(shí)施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍，在閱讀了本發(fā)明之后，本領(lǐng)域技術(shù)人員對(duì)本發(fā)明的各種等價(jià)形式的修改均落于本申請(qǐng)所附權(quán)利要求所限定的范圍。

本發(fā)明實(shí)施例公開的一種基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼的無人機(jī)組合導(dǎo)航方法為：通過慣性導(dǎo)航獲得無人機(jī)在導(dǎo)航坐標(biāo)系下的速度信息和位置信息，作為組合導(dǎo)航參數(shù)的狀態(tài)信息；通過視覺導(dǎo)航獲得無人機(jī)在導(dǎo)航坐標(biāo)系下的沿水平方向上的速度信息和飛行高度信息，作為組合導(dǎo)航參數(shù)的觀測(cè)信息；通過擴(kuò)展卡爾曼濾波將非線性系統(tǒng)線性化，并利用自適應(yīng)區(qū)間卡爾曼濾波將時(shí)變的系統(tǒng)參數(shù)建模為自適應(yīng)的區(qū)間模型，構(gòu)建組合導(dǎo)航濾波模型，融合慣性、視覺導(dǎo)航參數(shù)，對(duì)系統(tǒng)導(dǎo)航參數(shù)做出最優(yōu)估計(jì)。

如圖1所示，本發(fā)明實(shí)施例公開的一種基于擴(kuò)展自適應(yīng)區(qū)間卡爾曼的無人機(jī)組合導(dǎo)航裝置，包括微機(jī)電系統(tǒng)慣性測(cè)量單元、超聲波傳感器、視覺傳感器和數(shù)字信號(hào)處理模塊；數(shù)字信號(hào)處理模塊包括：慣性導(dǎo)航解算單元，視覺導(dǎo)航解算單元，以及濾波算法單元。本例中微機(jī)電系統(tǒng)慣性測(cè)量單元采用型號(hào)為mpu9250的傳感器，其內(nèi)包括三軸陀螺，三軸加速度計(jì)，三軸磁力計(jì)，數(shù)字信號(hào)處理模塊采用型號(hào)為tms320c5509的dsp，視覺傳感器型號(hào)為mt9v034，超聲波傳感器型號(hào)為mb1043。本發(fā)明的用于無人機(jī)的組合導(dǎo)航方法先利用微機(jī)電系統(tǒng)慣性測(cè)量單元獲取無人機(jī)的角運(yùn)動(dòng)和線運(yùn)動(dòng)速率再通過擴(kuò)展卡爾曼方法線性化系統(tǒng)模型，從而解算出無人機(jī)的系統(tǒng)導(dǎo)航參數(shù)的狀態(tài)信息；再使用視覺傳感器獲取無人機(jī)外界環(huán)境的圖像信息，通過針孔成像模型并結(jié)合超聲波傳感器所提供的高度信息解算出無人機(jī)的視覺導(dǎo)航參數(shù)；最后利用自適應(yīng)區(qū)間建模方法，在線調(diào)整狀態(tài)噪聲和觀測(cè)噪聲，并融合系統(tǒng)導(dǎo)航參數(shù)的狀態(tài)信息和觀測(cè)信息，得到系統(tǒng)導(dǎo)航參數(shù)的最優(yōu)估計(jì)，從而實(shí)現(xiàn)在溫度變化、運(yùn)載體頻繁的姿態(tài)變化、光照條件不理想，或者視覺傳感器定焦困難等復(fù)雜、惡劣情形下的導(dǎo)航定位任務(wù)。具體步驟如下：

(1)使用微機(jī)電系統(tǒng)慣性測(cè)量單元獲取無人機(jī)的角運(yùn)動(dòng)和線運(yùn)動(dòng)速率，由此建立基于擴(kuò)展卡爾曼濾波算法的慣性導(dǎo)航參數(shù)的數(shù)學(xué)計(jì)算平臺(tái)，從而解算出無人機(jī)的系統(tǒng)導(dǎo)航參數(shù)的狀態(tài)信息(姿態(tài)、速度、位置)。通過對(duì)陀螺儀進(jìn)行采樣，得到無人機(jī)的旋轉(zhuǎn)角速率運(yùn)動(dòng)信息，并采用四元數(shù)算法來解算無人機(jī)的姿態(tài)信息，具體方法如下：

記q為轉(zhuǎn)動(dòng)四元數(shù)來表示載體坐標(biāo)系(b系)相對(duì)于導(dǎo)航坐標(biāo)系(n系)的轉(zhuǎn)動(dòng)關(guān)系，利用四元數(shù)乘法并考慮到四元數(shù)微分方程則有：

其中是載體系相對(duì)于導(dǎo)航系的角速度矢量，q^*為四元數(shù)q的共軛復(fù)數(shù)。將上式展開為成矩陣的形式，并展開，有：

其中表示載體系相對(duì)于導(dǎo)航系的角速度在x,y,z軸上的分量,q0,q1,q2,q3分別為四元數(shù)q的四維分量(q＝q0+q1i+q2j+q3k)。

四元數(shù)與載體系到導(dǎo)航系的姿態(tài)矩陣存在變換關(guān)系，有：

所以，載體系相對(duì)于導(dǎo)航系包含三個(gè)姿態(tài)角的姿態(tài)向量可描述為：

速度更新的微分方程可以表示為：

其中f^b為通過對(duì)加速度計(jì)進(jìn)行采樣的比力，gⁿ為導(dǎo)航坐標(biāo)系下的重力加速度。將式(5)在三個(gè)方向上展開，有：

其中分別表示無人機(jī)在導(dǎo)航坐標(biāo)系內(nèi)速度沿x,y,z三個(gè)軸的分量；為地球自轉(zhuǎn)角速度的三個(gè)分量；為載體的位置速率，由載體在導(dǎo)航坐標(biāo)系的位置決定。

無人機(jī)的位置微分方程可表示為

其中λ,l,h分別表示無人機(jī)當(dāng)前位置的經(jīng)度、維度、高度；rm,rn分別為沿地球卯酉圈和子午圈的曲率半徑，而載體的三維速度則可通過之前敘述的速度更新算法獲得。

(2)使用視覺傳感器獲取無人機(jī)外界環(huán)境的圖像信息，通過灰度匹配的方法，觀察、計(jì)算在不同幀圖像中物體灰度模式的表觀運(yùn)動(dòng)，從中提取物體的視覺導(dǎo)航參數(shù)，并結(jié)合超聲波計(jì)算的無人機(jī)飛行的高度信息作為系統(tǒng)導(dǎo)航參數(shù)的觀測(cè)信息。具體方法如下：

考慮如圖2所示的針孔成像模型，設(shè)pc＝[xc,yc,zc]^t是在視覺傳感器坐標(biāo)系fc下的一個(gè)點(diǎn)，于是這一點(diǎn)在視覺傳感器成像平面下表示為p＝[x,y,l]^t，其中l(wèi)表示視覺傳感器的焦距。無人機(jī)在飛行過程中，點(diǎn)pc相對(duì)于視覺傳感器存在平動(dòng)運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)，于是點(diǎn)pc相對(duì)于fc的運(yùn)動(dòng)可以表示為

v＝-t-ω×pc(8)

式中t，ω分別為點(diǎn)pc的線運(yùn)動(dòng)和角運(yùn)動(dòng)。對(duì)式兩邊微分并整理可得光流矢量

vx、vy是光流矢量在x方向和y方向上的分量，可以通過塊匹配最小誤差絕對(duì)和的方法計(jì)算得出；式中的zc可以由超聲波傳感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺儀得出。于是根據(jù)式(9)和式(10)就可以計(jì)算得到無人機(jī)在導(dǎo)航坐標(biāo)系下的水平方向上的速度。

(3)利用區(qū)間概念將狀態(tài)方程和觀測(cè)方程的噪聲統(tǒng)計(jì)特性的不確定性建為區(qū)間模型，并利用自適應(yīng)模型動(dòng)態(tài)調(diào)整區(qū)間的范圍，在此基礎(chǔ)上再通過卡爾曼濾波方法融合系統(tǒng)導(dǎo)航參數(shù)的狀態(tài)信息和觀測(cè)信息，從而得到系統(tǒng)導(dǎo)航參數(shù)的最優(yōu)估計(jì)。具體為：

設(shè)系統(tǒng)的狀態(tài)方程和觀測(cè)方程分別為：

xk＝f[xk-1,k]+bkuk+wkqk(11)

zk＝h[xk,k]+vkrk(12)

式中：

是狀態(tài)向量，它包括無人機(jī)在導(dǎo)航坐標(biāo)系下的速度信息和位置信息，其中xn,yn,zn分別表示無人機(jī)在導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的位置信息，分別表示無人機(jī)在導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的速度信息。

zk＝[vxvyzc]^t是觀測(cè)向量，vx,vy分別表示無人機(jī)在攝像頭坐標(biāo)系下沿x,y兩個(gè)軸即水平方向上的光流速度信息，zc表示超聲波傳感器的讀數(shù)即無人機(jī)的飛行高度信息。

是系統(tǒng)的控制向量，由加速度計(jì)的數(shù)值經(jīng)過坐標(biāo)系矩陣變換得出，其中分別表示導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的無人機(jī)加速度信息。

bk＝[0dt0dt0dt]是控制分配矩陣,dt是采樣時(shí)間間隔。

wk＝[111111]是狀態(tài)噪聲分配矩陣。

qk＝diag[dxdvxdydvydzdvz]^2是狀態(tài)噪聲矩陣，dx、dvx、dy、dvy、dz、dvz分別是各狀態(tài)噪聲的標(biāo)準(zhǔn)差，它們與狀態(tài)向量中的元素一一對(duì)應(yīng)。

vk＝[111]是觀測(cè)噪聲分配矩陣。

rk＝diag[dfxdfydsz]^2是測(cè)量噪聲矩陣，dfx、dfy、dsz分別是各測(cè)量噪聲的標(biāo)準(zhǔn)差，它們與觀測(cè)向量中的元素一一對(duì)應(yīng)。

f[xk-1,k]代表k時(shí)刻下，系統(tǒng)關(guān)于xk-1這一點(diǎn)的狀態(tài)函數(shù)。

h[xk,k]代表k時(shí)刻下，系統(tǒng)關(guān)于xk這一點(diǎn)的觀測(cè)函數(shù)。

根據(jù)步驟(1)和步驟(2)所得到的慣性導(dǎo)航計(jì)算方程和視覺導(dǎo)航計(jì)算方程可通過泰勒展開的方法得到狀態(tài)模型和觀測(cè)模型的雅克比矩陣。

為狀態(tài)方程的雅克比矩陣；

為觀測(cè)方程的雅克比矩陣。

假設(shè)狀態(tài)噪聲陣和觀測(cè)噪聲陣在一定范圍內(nèi)變化：

式中與分別為狀態(tài)噪聲矩陣和觀測(cè)噪聲矩陣的區(qū)間模型，δqk與δrk是噪聲矩陣的變化范圍。稱為殘差向量，它表示卡爾曼濾波模型的準(zhǔn)確性。本發(fā)明中區(qū)間模型的自適應(yīng)的實(shí)現(xiàn)原理是通過計(jì)算殘差的實(shí)際量測(cè)方差nk與理論方差tk的矩陣的跡之比ε來對(duì)濾波模型的準(zhǔn)確性作出評(píng)價(jià)。當(dāng)模型準(zhǔn)確時(shí)，ε應(yīng)該在1附近；當(dāng)模型估計(jì)偏差較大時(shí)ε將偏離1，此時(shí)應(yīng)適當(dāng)修正觀測(cè)噪聲。ak為平滑因子，目的是為了減弱噪聲干擾對(duì)實(shí)際量測(cè)方差nk的影響，使數(shù)據(jù)更為平滑。τ是自適應(yīng)函數(shù)的比例因子c為動(dòng)態(tài)觀測(cè)噪聲的基準(zhǔn)矩陣。

那么完整的組合導(dǎo)航濾波模型可由式(18)-式(22)所表示：

其中，與分別為k時(shí)刻系統(tǒng)狀態(tài)向量的最優(yōu)估計(jì)和先驗(yàn)估計(jì)，為k-1時(shí)刻系統(tǒng)狀態(tài)向量的最優(yōu)估計(jì)；kk為k時(shí)刻卡爾曼濾波增益；與pk分別為k時(shí)刻一步預(yù)測(cè)誤差方差矩陣和估計(jì)誤差方差矩陣，pk-1則為k-1時(shí)刻的估計(jì)誤差方差矩陣；f代表系統(tǒng)狀態(tài)函數(shù)；bk是系統(tǒng)的控制分配矩陣；uk是系統(tǒng)的控制向量；ak為狀態(tài)方程的雅克比矩陣；hk為觀測(cè)方程的雅克比矩陣；wk是狀態(tài)噪聲分配矩陣；vk是觀測(cè)噪聲分配矩陣；zk是觀測(cè)向量；與分別為狀態(tài)噪聲矩陣和觀測(cè)噪聲矩陣的區(qū)間模型；上標(biāo)i為區(qū)間矩陣標(biāo)識(shí)，表示該矩陣向量為區(qū)間矩陣向量；表示觀測(cè)噪聲為零時(shí)，關(guān)于先驗(yàn)估計(jì)的系統(tǒng)觀測(cè)函數(shù)。

本發(fā)明的組合導(dǎo)航方法，融合擴(kuò)展卡爾曼濾波和自適應(yīng)區(qū)間卡爾曼濾波等算法優(yōu)點(diǎn)，比傳統(tǒng)單一的卡爾曼組合導(dǎo)航模型具有較強(qiáng)的魯棒性和環(huán)境適應(yīng)性，能夠得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。