專利名稱：一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種濾波裝置，更特別地說，是指一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)(MIMU/GPS)的分布式濾波裝置。

背景技術(shù)：
微慣性測量系統(tǒng)(MIMU)是由微機(jī)械陀螺、微機(jī)械加速度計(jì)和集成電路組成的慣性導(dǎo)航系統(tǒng)，具有完全自主性、不受外界干擾、短期精度高及數(shù)據(jù)更新快的優(yōu)點(diǎn)，且成本低、尺寸小、重量輕、功耗低。不足之處在于精度低、系統(tǒng)誤差隨時間積累，難以勝任長時間導(dǎo)航任務(wù)。
全球定位系統(tǒng)(GPS)是一種衛(wèi)星導(dǎo)航系統(tǒng)，具有定位精度高、長期穩(wěn)定性的優(yōu)點(diǎn)。不足之處在于自主性差、信號易失鎖、采樣率偏低，無法應(yīng)用在高動態(tài)、高干擾環(huán)境中。
MIMU/GPS組合導(dǎo)航系統(tǒng)將MIMU與GPS組合，利用GPS的長期穩(wěn)定性和高精度彌補(bǔ)MIMU誤差隨時間積累的缺點(diǎn)，利用MIMU的短期高精度彌補(bǔ)GPS系統(tǒng)易受干擾、信號易失鎖的缺點(diǎn)。其實(shí)質(zhì)是將MIMU的速度誤差、姿態(tài)誤差、位置誤差和器件誤差作為狀態(tài)量，將GPS提供的速度作為觀測量，通過信息融合技術(shù)估計(jì)出MIMU的各種誤差，然后進(jìn)行反饋校正。
在MIMU/GPS組合導(dǎo)航系統(tǒng)中，常采用的信息融合技術(shù)為卡爾曼濾波。卡爾曼濾波技術(shù)分為集中式(centralized)和分散式(decentralized)兩類。集中式卡爾曼濾波將各子系統(tǒng)的觀測數(shù)據(jù)送至信息融合中心進(jìn)行集中處理，理論上講，該方法可以得到嚴(yán)格的最優(yōu)狀態(tài)估計(jì)，缺點(diǎn)是對處理器的要求高、數(shù)據(jù)量大、實(shí)時計(jì)算和控制困難、可靠性較低、容錯性差；當(dāng)濾波維數(shù)較大時，在實(shí)際應(yīng)用中難以實(shí)現(xiàn)。在分散式卡爾曼濾波中，各局部濾波器利用相應(yīng)子系統(tǒng)的觀測值，得到局部狀態(tài)最優(yōu)估計(jì)，而后備局部估計(jì)送至融合中心，融合中心將局部估計(jì)融合成全局估計(jì)。分散式卡爾曼濾波由于采用了多處理器并行處理的分層結(jié)構(gòu)方式，因此計(jì)算量小、可靠性高、容錯性好，也便于實(shí)現(xiàn)系統(tǒng)多層次故障檢測與診斷。通過信息分配原理，將整個系統(tǒng)中的狀態(tài)信息分配到多個局部卡爾曼濾波器中，通過局部濾波器和主濾波器的逐步濾波，形成了聯(lián)邦卡爾曼濾波。聯(lián)邦卡爾曼濾波使得系統(tǒng)在降低運(yùn)算量和提高容錯能力的同時，濾波器的設(shè)計(jì)更為靈活，同時聯(lián)邦卡爾曼濾波的濾波結(jié)果在理論上和集中式卡爾曼濾波等價。然而聯(lián)邦卡爾曼濾波中信息分配因子的確定是一個難點(diǎn)，如果分配不當(dāng)，則會影響濾波結(jié)果。
前述濾波器的設(shè)計(jì)往往是基于一個參數(shù)固定的系統(tǒng)模型，但由于運(yùn)動載體機(jī)動、外部擾動以及環(huán)境條件的變化，系統(tǒng)模型參數(shù)會隨之變化，這時具有固定模型的常規(guī)濾波器難以跟隨參數(shù)的實(shí)際變化，使得濾波性能下降，針對此問題出現(xiàn)了多模型(Multiple Model，MM)估計(jì)器。MM方法選擇或設(shè)計(jì)一個模型集來描述系統(tǒng)可能的行為模式，系統(tǒng)的總體估計(jì)是多個并行運(yùn)行的卡爾曼濾波器估計(jì)的聯(lián)合，其中每個濾波器與特定的系統(tǒng)模式相匹配。在現(xiàn)有實(shí)際可行的MM方法中，交互式多模型(Interactive Multiple Model，IMM)方法被認(rèn)為是迄今為止最有效的多模型方法之一，其性能和計(jì)算量比達(dá)到了最優(yōu)。但是MM方法要求多個模型并行運(yùn)行，計(jì)算量較大，在低成本組合導(dǎo)航系統(tǒng)中，受硬件處理能力的限制，使得MM方法的應(yīng)用受阻。
另外，微機(jī)械陀螺和微機(jī)械加速度計(jì)的精度較低，用高階卡爾曼濾波器進(jìn)行較精確的建模會產(chǎn)生較大的模型誤差，同時，模型階數(shù)的增加導(dǎo)致計(jì)算量呈三次方上升，限制了工程應(yīng)用。


發(fā)明內(nèi)容
本發(fā)明針對低成本MIMU/GPS組合導(dǎo)航系統(tǒng)提出了一種分布式濾波的裝置，該濾波裝置能夠減小系統(tǒng)的計(jì)算量，以及高容錯能力，同時通過實(shí)時估計(jì)陀螺漂移并進(jìn)行補(bǔ)償，提高了MIMU/GPS組合導(dǎo)航系統(tǒng)輸出信息的精度。
本發(fā)明的一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，MIMU進(jìn)行慣性導(dǎo)航解算后得到速度、姿態(tài)和位置信息，GPS提供航向和速度信息；該分布式濾波裝置中包含5個差值比較單元和5個濾波器；所述的5個差值比較單元是指A差值比較單元(11)、B差值比較單元(21)、C差值比較單元(31)、D差值比較單元(41)和E差值比較單元(51)；所述的5個濾波器是指一階東向速度濾波器(10)、一階北向速度濾波器(20)、七階姿態(tài)卡爾曼濾波器(30)、一階東向位置濾波器(40)和一階北向位置濾波器(50)；該分布式濾波裝置利用MIMU和GPS的航向、速度信息作為量測輸入進(jìn)行濾波，濾波所得到的信息反饋到MIMU中進(jìn)行校正，最后經(jīng)過校正后的速度、姿態(tài)和位置信息作為MIMU/GPS組合導(dǎo)航系統(tǒng)的輸出。
本發(fā)明分布式濾波裝置的優(yōu)點(diǎn)為 1、對采用低成本微慣性器件的MIMU/GPS組合導(dǎo)航系統(tǒng)應(yīng)用分布式濾波方式進(jìn)行信息處理，由于濾波器維數(shù)降低，減小了系統(tǒng)的計(jì)算量，增強(qiáng)了實(shí)時性。
2、采用分布式濾波能夠彌補(bǔ)MIMU誤差隨時間積累的缺點(diǎn)，增強(qiáng)MIMU與GPS進(jìn)行組合系統(tǒng)魯棒性和穩(wěn)定性。
3、采用分布式濾波能夠彌補(bǔ)MIMU/GPS組合導(dǎo)航系統(tǒng)中在GPS信號受干擾時能起到良好的估計(jì)效果，增強(qiáng)了適應(yīng)性； 4、在分布濾波過程中，對MIMU和GPS輸出的速度和位置信息進(jìn)行融合，有效地增強(qiáng)了濾波容錯性，補(bǔ)償了陀螺漂移，提高了導(dǎo)航精度。



圖1為MIMU/GPS組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)簡示圖。
圖2為本發(fā)明分布式濾波器的濾波原理圖。

具體實(shí)施例方式 參見圖1所示，MIMU/GPS組合導(dǎo)航系統(tǒng)包括有MIMU、GPS和分布式濾波器。MIMU進(jìn)行慣性導(dǎo)航解算后得到速度、姿態(tài)和位置信息。GPS提供航向和速度信息。分布式濾波裝置利用MIMU和GPS的航向、速度信息作為量測輸入進(jìn)行濾波，濾波所得到的信息反饋到MIMU中進(jìn)行校正，最后經(jīng)過校正后的速度、姿態(tài)和位置信息作為MIMU/GPS組合導(dǎo)航系統(tǒng)的最終輸出。
參見圖2所示，本發(fā)明設(shè)計(jì)的分布式濾波裝置，該濾波裝置中包含5個差值比較單元和5個濾波器。
所述的5個差值比較單元是指A差值比較單元11、B差值比較單元21、C差值比較單元31、D差值比較單元41和E差值比較單元51。
所述的5個濾波器是指一階東向速度濾波器10、一階北向速度濾波器20、七階姿態(tài)卡爾曼濾波器30、一階東向位置濾波器40和一階北向位置濾波器50。
(一)東向速度誤差 A差值比較單元11用于接收微慣性測量系統(tǒng)輸出的東向速度VEINS和全球定位系統(tǒng)輸出的東向速度VEGPS，并將VEINS與VEGPS的差值δVE輸出給一階東向速度濾波器10； 一階東向速度濾波器10一方面對接收的δVE依據(jù)預(yù)測/濾波關(guān)系進(jìn)行預(yù)測或者濾波判斷，若滿足預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F10處理后輸出東向速度誤差估計(jì)值

PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目。
在本發(fā)明中，將δVE作為觀測量即可搭建東向速度濾波器，濾波方程為式中

表示當(dāng)前時刻k的狀態(tài)估計(jì)值；

表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量。
本發(fā)明一階東向速度濾波器10的初始化參數(shù)如下狀態(tài)變量X0＝0，誤差協(xié)方差矩陣P0＝100。
在預(yù)測模式下，一階東向速度濾波器10輸出的東向速度誤差預(yù)測值

受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則東向速度濾波器的濾波方程為 (二)北向速度誤差 B差值比較單元21用于接收微慣性測量系統(tǒng)輸出的北向速度VNINS和全球定位系統(tǒng)輸出的北向速度VNGPS，并將VNINS與VNGPS的差值δVN輸出給一階北向速度濾波器20； 一階北向速度濾波器20一方面對接收的δVN依據(jù)預(yù)測/濾波關(guān)系進(jìn)行預(yù)測或者濾波判斷，若滿足預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F20處理后輸出北向速度誤差估計(jì)值

PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目。
在本發(fā)明中，將δVN作為觀測量即可搭建北向速度濾波器，濾波方程為式中

表示當(dāng)前時刻k的狀態(tài)估計(jì)值；

表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量。
本發(fā)明的北向速度濾波器初始化參數(shù)如下狀態(tài)變量X0＝0，誤差協(xié)方差矩陣P0＝100。
在預(yù)測模式下，一階北向速度濾波器20輸出的北向速度誤差預(yù)測值

受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則北向速度濾波器的濾波方程為 (三)姿態(tài)誤差角、陀螺漂移量 C差值比較單元31用于接收微慣性測量系統(tǒng)輸出的東向速度VEINS和北向速度VNINS，以及全球定位系統(tǒng)輸出的東向速度VEGPS和北向速度VNGPS，并將VEINS與VEGPS的差值δVE和VNINS與VNGPS的差值δVN輸出給七階姿態(tài)濾波器30； 在本發(fā)明中，七階姿態(tài)卡爾曼濾波器30中的狀態(tài)變量量測變量則狀態(tài)方程量測方程其中 和 ΦE表示東向誤差角；ΦN表示北向誤差角；ΦUP表示方位失準(zhǔn)角；εx表示X軸陀螺漂移量；εy表示Y軸陀螺漂移量；Re表示地球半徑；g表示重力加速度；fE表示東向等效比力；fN表示北向等效比力；τr表示陀螺漂移馬爾科夫過程的時間常數(shù)；w1表示加表引起的輸入噪聲的東向分量；w2表示加表引起的輸入噪聲的北向分量；w3表示X軸陀螺的輸入噪聲；w4表示Y軸陀螺的輸入噪聲；w5表示Z軸陀螺的輸入噪聲；w6表示X軸陀螺的馬爾科夫噪聲；w7表示Y軸陀螺的馬爾科夫噪聲；vE表示東向速度誤差噪聲；vN表示北向速度誤差噪聲。
七階姿態(tài)卡爾曼濾波器30對接收的δVE和δVN進(jìn)行卡爾曼濾波方法處理后輸出姿態(tài)誤差估計(jì)值和陀螺漂移估計(jì)值

表示東向誤差角估計(jì)值；

表示北向誤差角估計(jì)值；

表示方位失準(zhǔn)角估計(jì)值；

表示X軸陀螺漂移估計(jì)值；

表示Y軸陀螺漂移估計(jì)值。
本發(fā)明的七階姿態(tài)卡爾曼濾波器30初始化參數(shù)如下狀態(tài)變量誤差 協(xié)方差矩陣 (四)東向位置誤差 D差值比較單元41用于接收微慣性測量系統(tǒng)輸出的東向位置EINS和全球定位系統(tǒng)輸出的東向位置EGPS，并將EINS與EGPS的差值δE(δE＝EINS-EGPS)輸出給一階東向位置濾波器40； 一階東向位置濾波器40一方面對接收的δE依據(jù)位置預(yù)測/濾波關(guān)系V40＝{PDOP＞7 or SV＜7}進(jìn)行預(yù)測或者濾波判斷，若滿足位置預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足位置預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F40處理后輸出東向位置誤差估計(jì)值

PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目。
在本發(fā)明中，將δE作為觀測量即可搭建東向位置濾波器，濾波方程為式中

表示當(dāng)前時刻k的狀態(tài)估計(jì)值；

表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量。
本發(fā)明的東向位置濾波器初始化參數(shù)如下狀態(tài)變量X0＝0，誤差協(xié)方差矩陣P0＝10000。
在預(yù)測模式下，一階東向位置濾波器40輸出的東向位置誤差預(yù)測值

受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則東向位置濾波器的濾波方程為 (五)北向位置誤差 E差值比較單元51用于接收微慣性測量系統(tǒng)輸出的北向位置NINS和全球定位系統(tǒng)輸出的北向位置NGPS，并將NINS與NGPS的差值δN(δN＝NINS-NGPS)輸出給一階北向位置濾波器50； 一階北向位置濾波器50一方面對接收的δN依據(jù)位置預(yù)測/濾波關(guān)系V50＝{PDOP＞7 or SV＜7}進(jìn)行預(yù)測或者濾波判斷，若滿足位置預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足位置預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F50處理后輸出北向位置誤差估計(jì)值

PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目。
在本發(fā)明中，將δN作為觀測量即可搭建北向位置濾波器，濾波方程為式中

表示當(dāng)前時刻k的狀態(tài)估計(jì)值；

表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量。
本發(fā)明的北向位置濾波器初始化參數(shù)如下狀態(tài)變量X0＝0，誤差協(xié)方差矩陣P0＝10000。
在預(yù)測模式下，一階北向位置濾波器50輸出的北向位置誤差預(yù)測值

受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則北向位置濾波器的濾波方程為 采用本發(fā)明的分布式濾波方式對某型MIMU/GPS組合導(dǎo)航系統(tǒng)進(jìn)行輸出精度的仿真測試，測試結(jié)果表明，經(jīng)本發(fā)明的濾波處理后，MIMU/GPS組合導(dǎo)航系統(tǒng)的輸出精度提高了3％～8％，且自適合性強(qiáng)。
仿真條件載體位置10816°，北緯3415°，高度2000m，初始北向速度為90m/s，東向速度和北向速度為零，初始失準(zhǔn)角向上0116°，陀螺儀漂移3°/h，陀螺漂移白噪聲漂移均方根1°/h，加速度計(jì)零偏01001g，速度誤差為0.26m/s2。
本發(fā)明的一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，該分布式濾波裝置分別對MIMU和GPS輸出的各個信息首先進(jìn)行差值比較，然后對差值進(jìn)行濾波的方式，充分體現(xiàn)了分布濾波帶來的計(jì)算量的降低、容錯能力的提高等優(yōu)勢，保證了MIMU/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。
權(quán)利要求
1.一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，MIMU進(jìn)行慣性導(dǎo)航解算后得到速度、姿態(tài)和位置信息，GPS提供航向和速度信息，其特征在于該分布式濾波裝置中包含5個差值比較單元和5個濾波器；
所述的5個差值比較單元是指A差值比較單元(11)、B差值比較單元(21)、C差值比較單元(31)、D差值比較單元(41)和E差值比較單元(51)；
所述的5個濾波器是指一階東向速度濾波器(10)、一階北向速度濾波器(20)、七階姿態(tài)卡爾曼濾波器(30)、一階東向位置濾波器(40)和一階北向位置濾波器(50)；
該分布式濾波裝置利用MIMU和GPS的航向、速度信息作為量測輸入進(jìn)行濾波，濾波所得到的信息反饋到MIMU中進(jìn)行校正，最后經(jīng)過校正后的速度、姿態(tài)和位置信息作為MIMU/GPS組合導(dǎo)航系統(tǒng)的輸出。
2.根據(jù)權(quán)利要求1所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于A差值比較單元(11)用于接收微慣性測量系統(tǒng)輸出的東向速度VEINS和全球定位系統(tǒng)輸出的東向速度VEGPS，并將東向速度差值輸出給一階東向速度濾波器(10)；
一階東向速度濾波器(10)一方面對接收的δVE依據(jù)預(yù)測/濾波關(guān)系進(jìn)行預(yù)測或者濾波判斷，若滿足預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F10處理后輸出東向速度誤差估計(jì)值
PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目；所述的濾波方程為
表示當(dāng)前時刻k的狀態(tài)估計(jì)值；
表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量；
在預(yù)測模式下，一階東向速度濾波器(10)輸出的東向速度誤差預(yù)測值
受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則東向速度濾波器的濾波方程為
3.根據(jù)權(quán)利要求1所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于B差值比較單元(21)用于接收微慣性測量系統(tǒng)輸出的北向速度VNINS和全球定位系統(tǒng)輸出的北向速度VNGPS，并將北向速度差值輸出給一階北向速度濾波器(20)；
一階北向速度濾波器(20)一方面對接收的δVN依據(jù)預(yù)測/濾波關(guān)系進(jìn)行預(yù)測或者濾波判斷，若滿足預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F20處理后輸出北向速度誤差估計(jì)值
PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目；所述的濾波方程為
表示當(dāng)前時刻k的狀態(tài)估計(jì)值；
表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量；
在預(yù)測模式下，一階北向速度濾波器(20)輸出的北向速度誤差預(yù)測值
受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則北向速度濾波器的濾波方程為
4.根據(jù)權(quán)利要求1所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于C差值比較單元(31)用于接收微慣性測量系統(tǒng)輸出的東向速度VEINS和北向速度VNINS，以及全球定位系統(tǒng)輸出的東向速度VEGPS和北向速度VNGPS，并將東向速度差值和北向速度差值輸出給七階姿態(tài)濾波器(30)；
七階姿態(tài)卡爾曼濾波器(30)中的狀態(tài)變量量測變量則狀態(tài)方程量測方程其中
和
ΦE表示東向誤差角；ΦN表示北向誤差角；ΦUP表示方位失準(zhǔn)角；εx表示X軸陀螺漂移量；εy表示Y軸陀螺漂移量；Re表示地球半徑；g表示重力加速度；fE表示東向等效比力；fN表示北向等效比力；τr表示陀螺漂移馬爾科夫過程的時間常數(shù)；w1表示加表引起的輸入噪聲的東向分量；w2表示加表引起的輸入噪聲的北向分量；w3表示X軸陀螺的輸入噪聲；w4表示Y軸陀螺的輸入噪聲；w5表示Z軸陀螺的輸入噪聲；w6表示X軸陀螺的馬爾科夫噪聲；w7表示Y軸陀螺的馬爾科夫噪聲；vE表示東向速度誤差噪聲；vN表示北向速度誤差噪聲；
七階姿態(tài)卡爾曼濾波器(30)對接收的δVE和δVN進(jìn)行卡爾曼濾波方法處理后輸出姿態(tài)誤差估計(jì)值和陀螺漂移估計(jì)值
表示東向誤差角估計(jì)值；
表示北向誤差角估計(jì)值；
表示方位失準(zhǔn)角估計(jì)值；
表示X軸陀螺漂移估計(jì)值；
表示Y軸陀螺漂移估計(jì)值。
5.根據(jù)權(quán)利要求1所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于D差值比較單元(41)用于接收微慣性測量系統(tǒng)輸出的東向位置EINS和全球定位系統(tǒng)輸出的東向位置EGPS，并將東向位置差值δE＝EINS-EGPS輸出給一階東向位置濾波器(40)；
一階東向位置濾波器(40)一方面對接收的δE依據(jù)位置預(yù)測/濾波關(guān)系V40＝{PDOP＞7 or SV＜7}進(jìn)行預(yù)測或者濾波判斷，若滿足位置預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足位置預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F40處理后輸出東向位置誤差估計(jì)值
PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目；所述的濾波方程
表示當(dāng)前時刻k的狀態(tài)估計(jì)值；
表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量；
在預(yù)測模式下，一階東向位置濾波器(40)輸出的東向位置誤差預(yù)測值
受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則東向位置濾波器的濾波方程為
6.根據(jù)權(quán)利要求1所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于E差值比較單元(51)用于接收微慣性測量系統(tǒng)輸出的北向位置NINS和全球定位系統(tǒng)輸出的北向位置NGPS，并將北向位置差值δN＝NINS-NGPS輸出給一階北向位置濾波器(50)；
一階北向位置濾波器(50)一方面對接收的δN依據(jù)位置預(yù)測/濾波關(guān)系V50＝{PDOP＞7 or SV＜7}進(jìn)行預(yù)測或者濾波判斷，若滿足位置預(yù)測/濾波關(guān)系則進(jìn)行預(yù)測處理；若不滿足位置預(yù)測/濾波關(guān)系則進(jìn)行濾波處理；另一方面，濾波時采用濾波方程F50處理后輸出北向位置誤差估計(jì)值
PDOP表示位置精度強(qiáng)弱度，SV表示GPS可見星的數(shù)目；所述的濾波方程
表示當(dāng)前時刻k的狀態(tài)估計(jì)值；
表示前一時刻k-1的狀態(tài)估計(jì)值；Pk表示當(dāng)前時刻k的誤差協(xié)方差矩陣；Pk-1表示前一時刻k-1的協(xié)方差矩陣；Q表示MIMU/GPS組合導(dǎo)航系統(tǒng)下的噪聲矩陣；R表示量測噪聲矩陣；Zk表示當(dāng)前時刻k的量測變量；
在預(yù)測模式下，一階北向位置濾波器(50)輸出的北向位置誤差預(yù)測值
受噪聲方差和濾波增益的影響，即噪聲方差趨于無窮大，濾波增益近似為零，則北向位置濾波器的濾波方程為
7.根據(jù)權(quán)利要求2或3或5或6所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于一階東向速度濾波器(10)、一階東向位置濾波器(40)、一階北向速度濾波器(20)和一階北向位置濾波器(50)的初始化參數(shù)中狀態(tài)變量X0＝0，誤差協(xié)方差矩陣P0＝100。
8.根據(jù)權(quán)利要求4所述的適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，其特征在于七階姿態(tài)卡爾曼濾波器(30)的初始化參數(shù)中狀態(tài)變量誤差協(xié)方差矩陣
全文摘要
本發(fā)明公開了一種適用于微慣性與全球定位組合導(dǎo)航系統(tǒng)的分布式濾波裝置，該濾波裝置中包含5個差值比較單元和5個濾波器。所述的差值比較單元和濾波器分別對MIMU和GPS輸出的航向、速度信息作為量測輸入進(jìn)行濾波，濾波所得到的信息反饋到MIMU中進(jìn)行校正，最后經(jīng)過校正后的速度、姿態(tài)和位置信息作為MIMU/GPS組合導(dǎo)航系統(tǒng)的最終輸出。本發(fā)明濾波裝置能夠減小系統(tǒng)的計(jì)算量，以及高容錯能力，同時通過實(shí)時估計(jì)陀螺漂移并進(jìn)行補(bǔ)償，提高了MIMU/GPS組合導(dǎo)航系統(tǒng)輸出信息的精度。
文檔編號G01C21/20GK101769743SQ20101003360
公開日2010年7月7日 申請日期2010年1月4日 優(yōu)先權(quán)日2010年1月4日
發(fā)明者富立, 劉志平, 高鵬, 閆立偉, 張宣, 宋偉 申請人:北京航空航天大學(xué)