自適應(yīng)濾波和聯(lián)邦卡爾曼濾波

2024/11/241測(cè)繪學(xué)院五系

自適應(yīng)濾波和聯(lián)邦卡爾曼濾波組合導(dǎo)航2024/11/242本次課主要內(nèi)容主要內(nèi)容: 卡爾曼濾波自適應(yīng)卡爾曼濾波 聯(lián)邦卡爾曼濾波重點(diǎn)和難點(diǎn): 自適應(yīng)卡爾曼濾波2024/11/243卡爾曼濾波是一種遞推線性最小方差估計(jì)。一、卡爾曼濾波設(shè)離散化后的系統(tǒng)狀態(tài)方程和量測(cè)方程分別為:4選取15階狀態(tài)變量平臺(tái)誤差角速度誤差位置誤差陀螺隨機(jī)常值漂移加速度計(jì)隨機(jī)常值偏置2024/11/244一、卡爾曼濾波5系統(tǒng)狀態(tài)方程狀態(tài)轉(zhuǎn)移矩陣噪聲轉(zhuǎn)移矩陣2024/11/245W系統(tǒng)噪聲向量一、卡爾曼濾波6系統(tǒng)量測(cè)方程觀測(cè)量由GNSS和INS獲得的位置速度誤差2024/11/246一、卡爾曼濾波2024/11/247系統(tǒng)量測(cè)方程一、卡爾曼濾波2024/11/248間接法濾波示意圖輸出校正用導(dǎo)航參數(shù)誤差的估值去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到綜合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值即：間接法卡爾曼濾波器:將慣性系統(tǒng)和其他的導(dǎo)航系統(tǒng)各自計(jì)算的導(dǎo)航參數(shù)進(jìn)行比較，其差值就包含了慣導(dǎo)某些導(dǎo)航參數(shù)誤差，即:一、卡爾曼濾波2024/11/249離散卡爾曼濾波方程或狀態(tài)一步預(yù)測(cè)方程狀態(tài)估計(jì)計(jì)算方程濾波增益方程一步預(yù)測(cè)均方誤差方程估計(jì)均方誤差方程一、卡爾曼濾波2024/11/2410濾波計(jì)算回路增益計(jì)算回路一、卡爾曼濾波2024/11/2411（1）狀態(tài)一步預(yù)測(cè)方程Xk-1的卡爾曼濾波估值利用

計(jì)算得到的一步預(yù)測(cè)的均值為零一、卡爾曼濾波2024/11/2412（2）狀態(tài)估計(jì)方程計(jì)算估值Xk的方程。它是在一步預(yù)測(cè)Xk/k-1的基礎(chǔ)上,根據(jù)量測(cè)值Zk計(jì)算出來(lái)的上式就是通過(guò)計(jì)算新息，并左乘一個(gè)系數(shù)矩陣把估計(jì)出來(lái)，加到中，從而得到估值，稱為濾波增益矩陣一、卡爾曼濾波2024/11/2413（3）估計(jì)均方誤差方程Kk選取的標(biāo)準(zhǔn)就是卡爾曼濾波的估計(jì)準(zhǔn)則，也就是使得均方誤差陣最小，為求Kk,先推導(dǎo)估計(jì)均方誤差預(yù)測(cè)誤差與量測(cè)噪聲互不相關(guān)，故和與量測(cè)噪聲互不相關(guān)。一、卡爾曼濾波2024/11/2414（4）濾波增益方程Kk選取的原則是使得均方誤差陣最小；采用微分求極值的方法，即：將改為，得到的估計(jì)均方誤差標(biāo)以，其與的差值為，求出使為非負(fù)定的值就是使為最小的。

一、卡爾曼濾波2024/11/2415是用來(lái)估計(jì)的系數(shù)矩陣。

設(shè)狀態(tài)矢量和量測(cè)矢量都是一維的,且H=1,這說(shuō)明增益系數(shù)是預(yù)測(cè)均方誤差在它和量測(cè)噪聲方差之和中所占的比例。一、卡爾曼濾波增益矩陣的直觀意義2024/11/2416（5）一步預(yù)測(cè)均方誤差方程從下式可以看出,求Kk必須先求出Pk/k-1式中，為的估計(jì)誤差，可以看出一步預(yù)測(cè)均方誤差陣Pk/k-1是從估計(jì)均方誤差陣Pk-1轉(zhuǎn)移過(guò)來(lái)的，并且再加上系統(tǒng)噪聲方差的影響。的均方誤差陣，即：一、卡爾曼濾波2024/11/2417二、自適應(yīng)卡爾曼濾波在卡爾曼濾波計(jì)算中,可能出現(xiàn)發(fā)散的現(xiàn)象。當(dāng)量測(cè)值數(shù)目不斷增加時(shí),按濾波方程計(jì)算的估計(jì)均方誤差陣趨于零或趨于某一穩(wěn)態(tài)值,但估計(jì)值和實(shí)際狀態(tài)的偏差卻越來(lái)越大,使濾波器逐漸失去估計(jì)作用。這種現(xiàn)象稱為濾波器的發(fā)散。2024/11/2418設(shè)系統(tǒng)和量測(cè)為:狀態(tài)和量測(cè)都是標(biāo)量，和為互不相關(guān)的零均值白噪聲序列，并有：設(shè)，試求解狀態(tài)估計(jì)值，估計(jì)誤差方差以及真實(shí)誤差。在設(shè)計(jì)濾波器時(shí)誤認(rèn)為系統(tǒng)沒(méi)有噪聲，即系統(tǒng)模型為：二、自適應(yīng)卡爾曼濾波濾波發(fā)散例子2024/11/2419二、自適應(yīng)卡爾曼濾波計(jì)算過(guò)程中的舍入誤差引起。系統(tǒng)的數(shù)學(xué)模型或噪聲模型的統(tǒng)計(jì)特性不準(zhǔn)確,沒(méi)有能夠真實(shí)的反映系統(tǒng)的真實(shí)過(guò)程。濾波發(fā)散原因2024/11/2420二、自適應(yīng)卡爾曼濾波Sage-Husa自適應(yīng)濾波強(qiáng)跟蹤Kalman濾波改進(jìn)的自適應(yīng)濾波漸消記憶自適應(yīng)濾波平方根濾波幾種主要的自適應(yīng)濾波方法2024/11/2421二、自適應(yīng)卡爾曼濾波

Sage-Husa自適應(yīng)濾波利用觀測(cè)數(shù)據(jù)進(jìn)行遞推濾波的同時(shí),通過(guò)時(shí)變?cè)肼暯y(tǒng)計(jì)估值器,實(shí)時(shí)估計(jì)和修正系統(tǒng)噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性,從而達(dá)到降低模型誤差、抑制濾波發(fā)散、提高濾波精度的目的。2024/11/2422二、自適應(yīng)卡爾曼濾波1.利用標(biāo)準(zhǔn)的卡爾曼濾波方程計(jì)算出各個(gè)狀態(tài)的估計(jì)值Sage-Husa自適應(yīng)濾波2024/11/2423二、自適應(yīng)卡爾曼濾波2.計(jì)算系統(tǒng)噪聲和量測(cè)噪聲的均值和方差矩陣Sage-Husa自適應(yīng)濾波2024/11/2424二、自適應(yīng)卡爾曼濾波Sage-Husa自適應(yīng)濾波實(shí)時(shí)解算系統(tǒng)噪聲統(tǒng)計(jì)特性,計(jì)算精度較高；增加了計(jì)算量,實(shí)時(shí)性難以保證；計(jì)算噪聲方差易失去正定性,穩(wěn)定性和收斂性不能完全保證。2024/11/2425二、自適應(yīng)卡爾曼濾波強(qiáng)跟蹤Kalman濾波通過(guò)犧牲一定的精度換取濾波穩(wěn)定性將狀態(tài)估計(jì)一步預(yù)測(cè)均方誤差陣乘以加權(quán)系數(shù)2024/11/2426二、自適應(yīng)卡爾曼濾波強(qiáng)跟蹤Kalman濾波狀態(tài)發(fā)生突變跟蹤狀態(tài)變化2024/11/2427二、自適應(yīng)卡爾曼濾波

強(qiáng)跟蹤Kalman濾波結(jié)構(gòu)簡(jiǎn)單、跟蹤能力強(qiáng)、可靠性高；破壞了濾波器的最優(yōu)條件,濾波精度降低。2024/11/2428二、自適應(yīng)卡爾曼濾波改進(jìn)的自適應(yīng)濾波2024/11/2429二、自適應(yīng)卡爾曼濾波收斂性判據(jù):儲(chǔ)備系數(shù)量測(cè)估計(jì)誤差,即新息改進(jìn)的自適應(yīng)濾波2024/11/2430二、自適應(yīng)卡爾曼濾波卡爾曼濾波器發(fā)散,計(jì)算的協(xié)方差陣P逐漸趨于零,狀態(tài)估計(jì)過(guò)于依賴過(guò)去的數(shù)據(jù),這種情況下,常采取漸消記憶的卡爾曼濾波來(lái)克服發(fā)散。采用漸消記憶限制Kalman濾波器對(duì)舊數(shù)據(jù)的利用權(quán)重,以便充分利用現(xiàn)時(shí)的觀測(cè)數(shù)據(jù)。漸消記憶自適應(yīng)濾波2024/11/2431二、自適應(yīng)卡爾曼濾波一步預(yù)測(cè)估計(jì)誤差方程漸消因子S>1，使增大，進(jìn)而增大，使得新量測(cè)值在估計(jì)值中的權(quán)重增加，“過(guò)老”的量測(cè)值的作用相對(duì)地減少，從而抑制濾波發(fā)散。漸消記憶自適應(yīng)濾波算法2024/11/2432二、自適應(yīng)卡爾曼濾波平方根濾波就是計(jì)算和的平方根L稱為A的平方根平方根濾波不但能保證和的非負(fù)定性，而且在數(shù)值計(jì)算中，計(jì)算的字長(zhǎng)只須計(jì)算的字長(zhǎng)的一半，就能達(dá)到相同的精度。平方根濾波算法33利用卡爾曼濾波技術(shù)對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行最優(yōu)估計(jì)組合有兩種途徑:集中式濾波分散化濾波

利用一個(gè)卡爾曼濾波器來(lái)集中地處理所有導(dǎo)航子系統(tǒng)的信息三、聯(lián)邦卡爾曼濾波34濾波器子系統(tǒng)1參考系統(tǒng)時(shí)間更新最優(yōu)融合子系統(tǒng)2子系統(tǒng)N三、聯(lián)邦卡爾曼濾波集中式濾波器結(jié)構(gòu)35優(yōu)點(diǎn):(1)

可以給出最優(yōu)的誤差估計(jì)；(2)只有一個(gè)濾波器,結(jié)構(gòu)簡(jiǎn)單,工程中容易實(shí)現(xiàn)。缺點(diǎn):(1)狀態(tài)維數(shù)高,計(jì)算負(fù)擔(dān)重,帶來(lái)“維數(shù)災(zāi)難”,不利于濾波實(shí)時(shí)運(yùn)行；(2)容錯(cuò)性能差,不利于故障診斷,信息污染問(wèn)題。三、聯(lián)邦卡爾曼濾波集中式濾波器特點(diǎn)36利用卡爾曼濾波技術(shù)對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行最優(yōu)估計(jì)組合的兩種途徑:分散化濾波

利用子濾波器處理子系統(tǒng)的信息通過(guò)全局濾波器實(shí)現(xiàn)所有子系統(tǒng)的信息融合集中式濾波三、聯(lián)邦卡爾曼濾波37聯(lián)邦濾波器一般結(jié)構(gòu)LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合三、聯(lián)邦卡爾曼濾波38聯(lián)邦濾波要解決的問(wèn)題:濾波器的容錯(cuò)性能要好,濾波的精度要高；由局部濾波到全局濾波的融合算法要簡(jiǎn)單,計(jì)算量小。上述幾個(gè)性能要求是矛盾的。要容錯(cuò)性能好,有時(shí)就要犧牲一些精度。為了解決這幾個(gè)性能要求,聯(lián)邦濾波中用了“誤差上界”技術(shù)和“信息分配”原則,獲得最佳折中的性能。三、聯(lián)邦卡爾曼濾波2024/11/2439三、聯(lián)邦卡爾曼濾波若有N個(gè)局部狀態(tài)估計(jì)和相應(yīng)的估計(jì)誤差協(xié)方差陣，且各局部估計(jì)互不相關(guān)，即，則全局最優(yōu)估計(jì)可表示為:

若的估計(jì)精度差,即很大,則它在全局估計(jì)的貢獻(xiàn)就比較少。

各子濾波器的估計(jì)不相關(guān)時(shí)的融合算法其中40信息分配原則首先說(shuō)明什么是信息，系統(tǒng)中有兩類信息:（1）狀態(tài)運(yùn)動(dòng)方程的信息狀態(tài)方程的信息量是與系統(tǒng)噪聲的方差成反比的,因此可用Q-1表示狀態(tài)方程信息量；（2）量測(cè)方程的信息量測(cè)方程的信息可用量測(cè)噪聲協(xié)方差的逆R-1表示。三、聯(lián)邦卡爾曼濾波2024/11/2441三、聯(lián)邦卡爾曼濾波

將系統(tǒng)噪聲總的信息分配到各子濾波器和主濾波器中去，即信息守恒原理

信息分配原則2024/11/2442三、聯(lián)邦卡爾曼濾波由子濾波器與主濾波器合成的全局估計(jì)值及其相應(yīng)的估計(jì)誤差方差陣被放大為后再反饋到子濾波器,以重置子濾波器的估計(jì)值。

信息分配系數(shù)根據(jù)信息分配原則來(lái)確定,不同的值可獲得聯(lián)邦濾波器的不同結(jié)構(gòu)和不同特性。

信息分配原則2024/11/2443三、聯(lián)邦卡爾曼濾波聯(lián)邦濾波器的設(shè)計(jì)步驟:將子濾波器和主濾波器的初始估計(jì)誤差方差矩陣設(shè)置為組合導(dǎo)航系統(tǒng)初始估計(jì)誤差方差的倍，滿足信息守恒原則；將子濾波器和主濾波器的過(guò)程噪聲協(xié)方差矩陣設(shè)置為組合導(dǎo)航系統(tǒng)過(guò)程噪聲協(xié)方差矩陣的倍；各子濾波器處理自己的量測(cè)信息，獲得局部估計(jì)；在得到各子濾波器的局部估計(jì)和主濾波器的估計(jì)后，進(jìn)行最優(yōu)合成；用全局最優(yōu)濾波解來(lái)重置各子濾波器和主濾波器的濾波值和估計(jì)誤差方差矩陣。

2024/11/2444三、聯(lián)邦卡爾曼濾波根據(jù)信息分配策略不同，聯(lián)邦濾波算法有4種實(shí)現(xiàn)模式:零復(fù)位模式變比例模式無(wú)反饋模式融合—反饋模式

聯(lián)邦濾波器結(jié)構(gòu)45（1）零復(fù)位模式（零化式重置）LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合三、聯(lián)邦卡爾曼濾波46優(yōu)、缺點(diǎn)主濾波器分配到全部信息;故障檢測(cè)和隔離能力強(qiáng);減少了數(shù)據(jù)通訊量,計(jì)算簡(jiǎn)單,解算精度不高。（1）零復(fù)位模式（零化式重置）三、聯(lián)邦卡爾曼濾波47（2）變比例模式（有重置）LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合三、聯(lián)邦卡爾曼濾波48優(yōu)、缺點(diǎn)主濾波器與子濾波器之間平均分配信息；融合后全局濾波精度高,局部濾波因?yàn)橛腥譃V波反饋,精度也提高了；一個(gè)子系統(tǒng)發(fā)生故障后,主濾波器受污染,隔離后必須重新初始化主濾波器。（2）變比例模式（有重置）三、聯(lián)邦卡爾曼濾波49（3）無(wú)反饋模式（無(wú)重置）LF2LF1LFN主濾波器子系統(tǒng)2子系統(tǒng)1子系統(tǒng)N參考系統(tǒng)時(shí)間更新最優(yōu)融合三、聯(lián)邦卡爾曼濾波2024/1