基于模糊魯棒自適應(yīng)CKF算法的MEMS-IMU姿態(tài)估計

喬美英，高翼飛，李宛妮，姚文豪

（河南理工大學(xué) 電氣工程與自動化學(xué)院，焦作 454000）

隨著無人機、虛擬現(xiàn)實器件（Virtual Reality, VR）、可穿戴醫(yī)療設(shè)備等技術(shù)的不斷發(fā)展，低成本、小體積、低功耗的微機電系統(tǒng)（Micro-Electro-Mechanical System, MEMS）慣導(dǎo)設(shè)備也得到了廣泛的發(fā)展應(yīng)用[1,2]。其內(nèi)部慣性測量單元（Inertial Measurement Unit,IMU）是實現(xiàn)慣性導(dǎo)航的基本組成單元，根據(jù)MEMS-IMU系統(tǒng)不同傳感器的測量數(shù)據(jù)，通過算法融合并實時解算出的姿態(tài)參數(shù)，實現(xiàn)對載體的定位和跟蹤。因此快速、準(zhǔn)確地解算載體實時的姿態(tài)信息是實現(xiàn)慣性導(dǎo)航的關(guān)鍵。

目前對慣性導(dǎo)航系統(tǒng)姿態(tài)解算的算法有很多，其中最具有代表性的是其估計結(jié)果具有無偏、一致最優(yōu)的卡爾曼濾波（Kalman Filter, KF）及其擴展算法：擴展卡爾曼濾波（Extended Kalman Filter, EKF），無跡卡爾曼濾波（Unscented Kalman Filter, UKF）及容積卡爾曼濾波（Cubature Kalman filter, CKF）[3]等，均廣泛應(yīng)用于航空航天，船舶和汽車的自主導(dǎo)航或輔助導(dǎo)航等方面。

KF濾波及其擴展算法的無偏估計、一致最優(yōu)是在假設(shè)量測噪聲嚴(yán)格服從高斯分布的條件下成立的，但實際應(yīng)用中由于存在各種干擾情況，常常導(dǎo)致假設(shè)不成立，使估計結(jié)果達不到一致最優(yōu)。為消除這些干擾，文獻[4]針對目標(biāo)運動時出現(xiàn)理論模型和實際模型不匹配、模型失配的問題，在傳統(tǒng)漸消濾波的基礎(chǔ)上提出了一種多重漸消因子的強跟蹤CKF算法。郭士犖等[5]在多重漸消因子的強跟蹤CKF算法的基礎(chǔ)上引入χ2檢驗條件，使多重漸消因子的引入時機更加合理。徐博等[6]針對水下復(fù)雜環(huán)境，設(shè)計了一種基于馬氏距離的自適應(yīng)CKF算法，使用濾波新息序列的馬氏距離判斷系統(tǒng)模型失準(zhǔn)情況。文獻[7]則針對系統(tǒng)狀態(tài)躍變以及非高斯量測噪聲干擾等問題，提出了一種基于m估計的魯棒卡爾曼濾波器，根據(jù)Huber的m估計方法重新定義了新息序列，使濾波的魯棒性得到提升。文獻[8]則借鑒Huber等價權(quán)函數(shù)的思想，構(gòu)造了逼近函數(shù)以抑制觀測野值的影響，提高了算法的魯棒性。但上述這些算法在處理濾波異常情況時，是獨立考慮算法的自適應(yīng)性和魯棒性，而所針對的也是僅單一異常情況。然而實際慣導(dǎo)系統(tǒng)的算法在不同環(huán)境下運行時，需要考慮多種干擾混合共存的情況，同時考慮算法的自適應(yīng)性和魯棒性。

基于此，本文提出了一種模糊魯棒自適應(yīng)容積卡爾曼濾波（Fuzzy Robust Adaptive - Cubature Kalman Filter, FRA-CKF）算法，借鑒濾波新息序列的2χ檢驗，根據(jù)2χ分布上分位點與假設(shè)檢驗原理設(shè)置了不同異常的修正門限，構(gòu)造了模糊修正邊界并提出了相應(yīng)的模糊修正準(zhǔn)則。將算法的自適應(yīng)性和魯棒性進行了有機融合，提高了濾波性能。實驗結(jié)果表明，本文所提出的算法在受到不同干擾時能夠及時對異常進行修正，準(zhǔn)確地估計系統(tǒng)姿態(tài)。

1 姿態(tài)估計模型與濾波算法

1.1 姿態(tài)估計模型

本文以IMU系統(tǒng)中的陀螺儀為對象，用四元數(shù)方式描述姿態(tài)參數(shù)，建立IMU系統(tǒng)的狀態(tài)方程。令四元數(shù)q為系統(tǒng)狀態(tài)，則根據(jù)陀螺儀的輸出角速度和四元數(shù)的微分方程有：

其中 q=[q0,q1, q2,q3]T且為單位四元數(shù)，Ω(ω)是由陀螺儀測量角速度構(gòu)成的矩陣，其矩陣形式為：

將式(1)進行離散化處理，以方便迭代計算：

其中 qk=[ q0,k, q1,k, q2,k, q3,k]T為k時刻的四元數(shù)系統(tǒng)狀態(tài)。由于陀螺儀在測量過程中會受噪聲和自身漂移影響，所以陀螺儀實際輸出的角速度應(yīng)包含這部分誤差。設(shè)陀螺儀所受干擾引起的角速度影響為 ωΔ ，則實際輸出的角速度應(yīng)為： ωs= ωt+Δω，其中 ωt為真實的角速度。將 ωs= ωt+Δω帶入式(3)有：

IMU的加速度計通過測量重力矢量能夠獲得載體的俯仰信息和橫滾信息，磁強計通過測量磁場矢量能夠得到載體的航向信息，所以本文選用加速度計和磁強計為系統(tǒng)的量測，對陀螺儀漂移進行修正。

加速度計的輸出模型為：

其中上標(biāo)b表示載體坐標(biāo)系(右前上)，上標(biāo)n表示導(dǎo)航坐標(biāo)系(東北天)；an=［ 0 0g］T為導(dǎo)航坐標(biāo)系下的重力加速度，為k時刻載體坐標(biāo)系下的加速度測量值，為k時刻從導(dǎo)航坐標(biāo)系(n系)到載體坐標(biāo)系(b系)的旋轉(zhuǎn)矩陣，加速度計噪聲面垂直，因此使用加速度計量測系統(tǒng)的俯仰角信息和由于重力平面始終和系統(tǒng)的航向平橫滾角信息，旋轉(zhuǎn)矩陣形式為：

將式(6)中第一項的計算結(jié)果拆分出四元數(shù)qk，得到k時刻的加速度計量測矩陣：

將式(8)帶入式(6)，可得加速度計輸出模型的線性形式：

磁強計的輸出模型為：

其中 hn= [ B · cosδ 0 -B ·sinδ]T為地磁場強度B在導(dǎo)航坐標(biāo)系的投影，δ為磁偏角，磁強計噪聲Vh,k~N(0, Rh,k)，且Wk，Va,k和Vh,k三種噪聲是相互獨立的。同理，將式(10)中第一項的計算結(jié)果拆分出四元數(shù)qk，可得矩陣：

其中 B1= B ·cosδ， B2= B ·sinδ。將式(11)帶入式(10)，可得磁強計輸出模型的線性形式：

聯(lián)立式(5)(9)(12)得到IMU系統(tǒng)的狀態(tài)方程和量測方程。

1.2 濾波算法

1）漸消自適應(yīng)濾波

KF及其擴展算法估計結(jié)果的無偏、最優(yōu)是在假設(shè)量測噪聲服從高斯白噪聲分布的前提下才成立的。系統(tǒng)量測值和量測預(yù)測值之間的殘差稱為新息kη，在此假設(shè)成立的前提下，濾波的新息序列kη服從均值為零，協(xié)方差理論值為的高斯序列。

當(dāng)量測受到非高斯噪聲干擾時假設(shè)不成立，濾波估計精度將下降，傳統(tǒng)方式是通過新息序列協(xié)方差的理論值和計算值進行判斷：非高斯噪聲影響時，新息序列協(xié)方差的實際計算值和理論值是不相等的，新息序列協(xié)方差估計值的計算式為：

漸消自適應(yīng)KF是一種抑制濾波新息異常的簡單有效方法[4]，如果新息序列為高斯序列，則濾波新息序列的馬氏距離應(yīng)服從卡方分布。根據(jù)假設(shè)檢驗原理和 χ2檢驗原理，選取以 χ2分布顯著性水平為α的上分位點作為濾波評判標(biāo)準(zhǔn)。例如，若量測為三維時，選取的上分位點= 11.345，通過查表得也就是說，在濾波正常的情況下，新息序列的馬氏距離大于的概率只有1%。根據(jù)假設(shè)檢驗原理，若新息序列的馬氏距離大于則在99%的置信度下可以認(rèn)為濾波異常。將此判斷方式應(yīng)用到CKF中，則有：

其中dm為新息序列的馬氏距離。

在正常噪聲情況下 dm＜，按照CKF濾波對系統(tǒng)狀態(tài)進行估計；當(dāng) dm＞時，對CKF算法進行修正，通過自適應(yīng)的方式調(diào)節(jié)這一現(xiàn)象：引入一個時變的標(biāo)量因子 kλ，僅針對該時刻的新息序列進行估計，對狀態(tài)預(yù)測誤差協(xié)方差Pk/k-1的第一項進行調(diào)整：

通過高斯牛頓法進行迭代求解，可以得到λk的遞推式[12]：

2）膨脹魯棒濾波

當(dāng)外部的測量信息不可靠時，通過提高量測的權(quán)重從而抑制模型失配帶來的誤差，當(dāng)量測含有較大噪聲時，若繼續(xù)使用漸消自適應(yīng)的方式進行調(diào)整，則會影響濾波估計的精度。文獻[9]提出了一種與漸消自適應(yīng)濾波類似的方法，在KF的基礎(chǔ)上引入一個時變的標(biāo)量膨脹因子對新息協(xié)方差的第二項進行調(diào)整，對量測噪聲進行膨脹，削弱量測對濾波的修正作用。

當(dāng) dm＞時，同樣以CKF濾波為對象，對新息序列協(xié)方差中的量測噪聲協(xié)方差R進行處理：

膨脹因子 μk也需要滿足：

同樣使用高斯牛頓法進行迭代求解，得到膨脹因子 μk的遞推式[10]：

其中膨脹因子 μk的初值為1。

與漸消自適應(yīng)濾波相反，膨脹魯棒濾波通過對量測噪聲的膨脹作用，減小了濾波增益，削弱了量測的權(quán)重，從而提高系統(tǒng)自身的魯棒性。

這兩種方法分別針對不同異常情況提出了各自的調(diào)整方式，自適應(yīng)濾波更加相信量測信息的修正作用，而魯棒濾波則更看重系統(tǒng)模型的預(yù)測信息。兩種調(diào)節(jié)濾波的方式，就像“杠桿”一樣，是互相對立的調(diào)節(jié)方式，同一時刻在濾波算法中難以實現(xiàn)平衡[11]。在實際應(yīng)用中，系統(tǒng)不可避免地會出現(xiàn)狀態(tài)不連續(xù)變化時的模型失準(zhǔn)，以及受外界干擾影響使量測含有非高斯噪聲的情況[12]。

因此，針對此情況提出了一種模糊修正準(zhǔn)則，將CKF、自適應(yīng)修正和魯棒修正相互貫通，將這根“杠桿”轉(zhuǎn)變?yōu)椤皹蛄骸?，使算法兼顧魯棒性和自適應(yīng)性。

2 基于模糊修正的魯棒自適應(yīng)CKF

2.1 修正門限值

新息序列的統(tǒng)計特性是判斷濾波是否異常的關(guān)鍵[9]。本文同樣利用2χ檢驗原理對濾波異常判斷。并將設(shè)置為修正起始門限，以二維平面為例展示修正域，設(shè)原點為圓心，以修正門限為半徑作圓如圖1所示(參數(shù)β＞0)。若k時刻沒有出現(xiàn)異常，新息序列的馬氏距離應(yīng)在小圓內(nèi)，此時無需修正；若濾波出現(xiàn)異常，新息序列的馬氏距離超出修正半徑，此時需要根據(jù)模糊修正準(zhǔn)則執(zhí)行相應(yīng)的修正。

圖1 修正門限圓Fig.1 Modifiedthreshold circle

2.2 模糊修正準(zhǔn)則

根據(jù)引起濾波異常的原因，需要使用不同方法進行修正，但是如果直接設(shè)置自適應(yīng)修正的邊界和魯棒修正的邊界，嚴(yán)格按照界內(nèi)界外進行修正，顯然是不合理的。如果dm出現(xiàn)在自適應(yīng)修正邊界內(nèi)但距離魯棒修正邊界很近的情況，或是在魯棒修正邊界內(nèi)但距離自適應(yīng)修正邊界很近的情況，嚴(yán)格按照界內(nèi)界外的標(biāo)準(zhǔn)可能會引起錯誤修正，出現(xiàn)相反的修正結(jié)果。

為避免這種現(xiàn)象發(fā)生，合理修正異常情況，本文提出了模糊修正準(zhǔn)則，通過將修正邊界模糊化把邊界轉(zhuǎn)化為域，在域內(nèi)通過設(shè)置不同修正方式的隸屬度函數(shù)，用隸屬程度這一概念將不修正、自適應(yīng)修正和魯棒修正三種修正方式融合，根據(jù)模糊修正準(zhǔn)則對濾波異常進行合理修正。模糊修正準(zhǔn)則具體由三部分組成：

1) 設(shè)置模糊邊界

在設(shè)置不同異常的模糊邊界時，考慮到系統(tǒng)狀態(tài)出現(xiàn)躍變導(dǎo)致模型失準(zhǔn)情況時，實際量測值和量測預(yù)測值之間的差異較大，濾波新息會大幅度變化；而系統(tǒng)受到噪聲干擾或累積誤差引起的新息變化相對較小。因此，在修正門限的基礎(chǔ)上選定4個參數(shù)a，b，c，d分別作為CKF、自適應(yīng)修正和魯棒修正邊界，其中

2) 構(gòu)造隸屬度函數(shù)

三種算法標(biāo)準(zhǔn)CKF、魯棒修正（RCKF）和自適應(yīng)修正（ACKF）的隸屬度函數(shù)分別為：

三種隸屬度函數(shù)的選取原因是：當(dāng)新息序列的馬氏距離超過修正門限且不斷增大時，不修正的隸屬度應(yīng)快速下降，所以選取了拋物線型為不修正的隸屬度函數(shù)；此時魯棒修正的隸屬度應(yīng)逐漸增大，因此選取了增長趨勢為線性的半梯形隸屬度函數(shù)。當(dāng)新息序列的馬氏距離增長過魯棒修正邊界時，魯棒修正的隸屬度也應(yīng)逐漸下降，若不選擇梯形而選擇拋物線型，則魯棒修正的程度會下降得過快從而使修正不到位。自適應(yīng)修正的隸屬度函數(shù)應(yīng)先緩慢增長再快速上升，因此自適應(yīng)修正的隸屬度函數(shù)選擇半拋物線型。以a=2,b= 4,c=6,d=8為例，三種修正的隸屬度函數(shù)如圖2所示。

圖2 三種修正的隸屬度函數(shù)Fig.2 Membership functions of three modified

3) 制定模糊修正準(zhǔn)則

根據(jù)隸屬度函數(shù)對濾波異常進行判斷并修正：當(dāng)dm在模糊域(0,a]之間時，濾波無異常，執(zhí)行標(biāo)準(zhǔn)CKF算法。當(dāng)dm在模糊域(a,b)之間時，濾波異常，執(zhí)行修正，此時既執(zhí)行CKF也執(zhí)行魯棒修正，但是隸屬于CKF的“程度”逐漸減小，隸屬于魯棒修正的“程度”逐漸增大。當(dāng)dm在模糊域[b,c)之間時，新息異常是濾波受到噪聲干擾引起的，應(yīng)對量測噪聲做出調(diào)整降低量測的修正作用，因此執(zhí)行魯棒修正。當(dāng)dm在模糊域[c,d)之間時，此時既執(zhí)行魯棒修正也執(zhí)行自適應(yīng)修正，但是隸屬于魯棒修正的“程度”逐漸減小，隸屬于自適應(yīng)修正的“程度”逐漸增大。當(dāng)dm超過模糊修正邊界d時，新息異常是由狀態(tài)躍變或模型失配引起的，應(yīng)對系統(tǒng)預(yù)測協(xié)方差進行調(diào)整，降低系統(tǒng)預(yù)測部分的權(quán)重，增大量測的修正作用，因此執(zhí)行自適應(yīng)修正。針對濾波異常時的模糊修正準(zhǔn)則如表1所示。整體流程如圖3所示。

表1 模糊修正準(zhǔn)則Tab.1 Fuzzy correction criterion

圖3 濾波異常判斷和模糊修正準(zhǔn)則Fig.3 Filter anomaly judgment and fuzzy correction criteri

3 仿真試驗與分析

3.1 仿真試驗

為了驗證所提模糊修正準(zhǔn)則在不同異常情況下對CKF算法的修正能力。本節(jié)在Matlab2018a環(huán)境下進行仿真試驗驗證，為了接近本文所用IMU系統(tǒng)的模型，設(shè)置系統(tǒng)的狀態(tài)方程為：

式中：k =1，2，3…n為迭代次數(shù)，x為系統(tǒng)狀態(tài)用四元數(shù)形式表示且初始值x(0)=[0,0,0,0]T，F(xiàn)為主對角線元素為1，其余元素為0.01的四階矩陣，I為四階單位矩陣，系統(tǒng)噪聲協(xié)方差和量測噪聲協(xié)方差分別為 Q = 10-4·I和 R = 10-1·I。量測噪聲在迭代的第40次時以指數(shù)形式擴大一倍后不變，仿真了系統(tǒng)在不同環(huán)境不同場合運行時的非高斯噪聲影響。在迭代至第40、41次時，給系統(tǒng)狀態(tài)施加一個強度為50的正向脈沖信號；在迭代至第80、81次時給系統(tǒng)狀態(tài)施加一個強度為150的反向脈沖信號；在迭代至第120-125時給系統(tǒng)施加強度為20的反向階躍信號，使系統(tǒng)狀態(tài)發(fā)生不連續(xù)躍變，仿真了系統(tǒng)運動狀態(tài)發(fā)生變化導(dǎo)致的模型失準(zhǔn)狀況。仿真結(jié)果如圖4所示（僅展示q0的仿真結(jié)果，其余元素與q0結(jié)果類似）。

圖4 q0的迭代結(jié)果Fig.4 Iteration result of q0

圖4 中本文所提的FRA-CKF算法有效地跟蹤了系統(tǒng)實際情況，在系統(tǒng)狀態(tài)發(fā)生躍變時做出了合理的自適應(yīng)修正，準(zhǔn)確地跟蹤系統(tǒng)狀態(tài)，并且在僅受噪聲影響，未發(fā)生躍變時，能夠進行魯棒修正、漸消噪聲影響，有效地驗證了所提模糊修正準(zhǔn)則。

為了進一步驗證模糊修正魯棒自適應(yīng)CKF算法準(zhǔn)確跟蹤系統(tǒng)狀態(tài)的能力，將該算法與漸消自適應(yīng)CKF算法（ACFK），魯棒CKF算法（RCFK）和CKF算法的估計結(jié)果進行對比。將四種算法的濾波參數(shù)設(shè)置相同，對系統(tǒng)狀態(tài)進行估計，結(jié)果如圖5所示。

圖5 不同濾波算法對比Fig.5 Comparison of different filtering algorithms

圖5 展示的濾波估計結(jié)果中能看出，所提FRA-CKF算法在系統(tǒng)狀態(tài)僅受噪聲干擾時的估計精度與魯棒CKF算法相當(dāng)，高于CKF算法，自適應(yīng)CKF算法的估計精度最低；然而當(dāng)系統(tǒng)狀態(tài)發(fā)生躍變時，魯棒CKF算法沒有跟蹤上系統(tǒng)的狀態(tài)，CKF算法雖然勉強跟蹤上了系統(tǒng)的狀態(tài)，但與實際系統(tǒng)狀態(tài)依然有較大的相差。只有自適應(yīng)CKF算法和FRA-CKF算法能夠跟蹤系統(tǒng)的變化，但自適應(yīng)CKF算法的估計精度受噪聲干擾，估計的結(jié)果振蕩不穩(wěn)定。只有FRA-CKF既能夠及時地跟蹤系統(tǒng)狀態(tài)，也能在噪聲干擾下具有較高的精度和穩(wěn)定的估計結(jié)果。

4 實測實驗

為驗證所提FRA-CKF算法在慣性導(dǎo)航姿態(tài)估計中的實際應(yīng)用效果，利用MEMS-IMU JY901進行實測實驗驗證。

1)靜態(tài)實驗：將JY901接通好固定在無磁雙軸轉(zhuǎn)臺上，使芯片x軸對準(zhǔn)北向。轉(zhuǎn)臺開機后轉(zhuǎn)置水平，在靜置1小時之后開始采集數(shù)據(jù)，采樣頻率為60 Hz，采樣時間為73 s，如圖6所示。

圖6 雙軸轉(zhuǎn)臺和帶有MEMS-IMU的測量載體Fig.6 Two axis turntable and carrier with MEMS-IMU

設(shè)置對比實驗：由于漸消自適應(yīng)CKF和魯棒CKF在濾波無異常時不執(zhí)行修正，與CKF算法為同一算法，而FRA-CKF算法通過設(shè)置修正邊界和模糊修正準(zhǔn)則在靜態(tài)條件下依然具有修正作用。所以靜態(tài)實驗僅與CKF算法進行對比。分別使用兩種算法對系統(tǒng)進行姿態(tài)估計，兩種算法的姿態(tài)估計結(jié)果如圖7-9所示。

圖7 俯仰角估計結(jié)果Fig.7 Pitch angle estimation results

圖8 橫滾角估計結(jié)果Fig.8 Roll angle estimation results

圖9 航向角估計結(jié)果Fig.9 Heading angle estimation results

從圖7-9中可以看出，在靜態(tài)條件下FRA-CKF算法相比CKF算法對姿態(tài)參數(shù)的估計精度更高，且波動范圍小，具有穩(wěn)定估計結(jié)果。為了進一步衡量兩種算法，表2給出了兩算法的均方根誤差。

表2 兩種算法的均方根誤差Tab.2 Root mean square error of two algorithms

從表2中能看出FRA-CKF算法相比CKF算法，靜態(tài)時姿態(tài)估計結(jié)果的均方根誤差，俯仰角降低了24%，橫滾角降低9%，航向角降低了80%，驗證了算法的準(zhǔn)確性。

2)動態(tài)實驗：對帶有MEMS-IMU JY901的測量載體進行車載實驗，將載體連接好后固定在汽車后座上，記錄汽車的運動姿態(tài)，如圖10所示。采樣頻率為60 Hz，采樣時間為13 min。行駛軌跡如圖11所示。

圖10 動態(tài)車載實驗Fig.10 Dynamic vehicle experiment

圖11 行駛軌跡Fig.11 Driving track

設(shè)置對比實驗：由于動態(tài)車載實驗測量數(shù)據(jù)會受外界干擾從而影響CKF算法，使算法出現(xiàn)異常情況。漸消自適應(yīng)CKF和魯棒CKF在異常情況將執(zhí)行相應(yīng)的修正，因此動態(tài)實驗將FRA-CKF、漸消自適應(yīng)CKF以及魯棒CKF算法進行對比，進而驗證算法的實用性能。分別使用三種算法對系統(tǒng)進行姿態(tài)估計，根據(jù)實測數(shù)據(jù)三種算法的姿態(tài)估計結(jié)果如圖12-14所示。

圖12 俯仰角估計結(jié)果Fig.12 Pitch angle estimation results

圖13 橫滾角估計結(jié)果Fig.13 Roll angle estimation results

圖14 航向角估計結(jié)果Fig.14 Heading angle estimation results

圖12-14中的姿態(tài)信息表明：漸消自適應(yīng)CKF的俯仰信息和橫滾信息隨著航向信息的變化受到了較大干擾，而魯棒CKF不隨航向信息的變化而變化，始終穩(wěn)定。但兩種算法解算的航向信息在多次變化后的解算精度嚴(yán)重下降，第630 s后解算的航向嚴(yán)重偏離實際運動情況。而FRA-CKF算法在航向多次發(fā)生變化后仍能準(zhǔn)確地估計，且俯仰信息和橫滾信息不敏感外界干擾，驗證了算法兼顧自適應(yīng)與魯棒性的特點和在實際應(yīng)用中的有效性。

5 結(jié) 論

針對慣性導(dǎo)航系統(tǒng)受到狀態(tài)突變、未知量測噪聲等干擾，傳統(tǒng)濾波算法因發(fā)散而無法準(zhǔn)確估計系統(tǒng)姿態(tài)的問題，提出了一種模糊魯棒自適應(yīng)容積卡爾曼濾波算法。通過設(shè)置起始修正門限對濾波新息的異常進行判斷，并根據(jù)誤差來源設(shè)置了修正邊界，構(gòu)造隸屬度函數(shù)提出模糊修正準(zhǔn)則。根據(jù)模糊修正準(zhǔn)則對狀態(tài)預(yù)測協(xié)方差或量測噪聲協(xié)方差修正，使算法兼顧魯棒性和自適應(yīng)性。實驗結(jié)果表明，該算法在靜態(tài)和動態(tài)條件下相比其他算法具有準(zhǔn)確、穩(wěn)定的姿態(tài)估計結(jié)果。