2023/8/211測繪學院五系

自適應濾波和聯邦卡爾曼濾波組合導航2023/8/51測繪學院五系自適應濾波和聯邦卡爾曼濾波組2023/8/212本次課主要內容主要內容： 卡爾曼濾波

自適應卡爾曼濾波 聯邦卡爾曼濾波重點和難點： 自適應卡爾曼濾波2023/8/52本次課主要內容主要內容：2023/8/213卡爾曼濾波是一種遞推線性最小方差估計。一、卡爾曼濾波設離散化后的系統狀態方程和量測方程分別為：2023/8/53卡爾曼濾波是一種遞推線性最小方差估計。一、4選取15階狀態變量平臺誤差角速度誤差位置誤差陀螺隨機常值漂移加速度計隨機常值偏置2023/8/214一、卡爾曼濾波4選取15階狀態變量平臺誤差角速度誤差位置誤差陀螺隨機常值漂5系統狀態方程狀態轉移矩陣噪聲轉移矩陣2023/8/215W系統噪聲向量一、卡爾曼濾波5系統狀態方程狀態轉移矩陣噪聲轉移矩陣2023/8/556系統量測方程觀測量由GNSS和INS獲得的位置速度誤差2023/8/216一、卡爾曼濾波6系統量測方程觀測量由GNSS和INS獲得的2023/8/2023/8/217系統量測方程一、卡爾曼濾波2023/8/57系統量測方程一、卡爾曼濾波2023/8/218間接法濾波示意圖輸出校正用導航參數誤差的估值去校正系統輸出的導航參數，得到綜合導航系統的導航參數估值即：間接法卡爾曼濾波器：將慣性系統和其他的導航系統各自計算的導航參數進行比較，其差值就包含了慣導某些導航參數誤差，即：一、卡爾曼濾波2023/8/58間接法濾波示意圖輸出校正用導航參數誤差的估2023/8/219離散卡爾曼濾波方程或狀態一步預測方程狀態估計計算方程濾波增益方程一步預測均方誤差方程估計均方誤差方程一、卡爾曼濾波2023/8/59離散卡爾曼濾波方程或狀態一步預測方程狀2023/8/2110濾波計算回路增益計算回路一、卡爾曼濾波2023/8/510濾波計算回路增益計算回路一、卡爾曼濾波2023/8/2111（1）狀態一步預測方程Xk-1的卡爾曼濾波估值利用

計算得到的一步預測的均值為零一、卡爾曼濾波2023/8/511（1）狀態一步預測方程Xk-1的卡爾曼濾2023/8/2112（2）狀態估計方程計算估值Xk的方程。它是在一步預測Xk/k-1的基礎上，根據量測值Zk計算出來的上式就是通過計算新息，并左乘一個系數矩陣把估計出來，加到中，從而得到估值，稱為濾波增益矩陣一、卡爾曼濾波2023/8/512（2）狀態估計方程計算估值Xk的方程。2023/8/2113（3）估計均方誤差方程Kk選取的標準就是卡爾曼濾波的估計準則，也就是使得均方誤差陣最小，為求Kk,先推導估計均方誤差預測誤差與量測噪聲互不相關，故和與量測噪聲互不相關。一、卡爾曼濾波2023/8/513（3）估計均方誤差方程Kk選取的2023/8/2114（4）濾波增益方程Kk選取的原則是使得均方誤差陣最小；采用微分求極值的方法，即：將改為，得到的估計均方誤差標以，其與的差值為，求出使為非負定的值就是使為最小的。

一、卡爾曼濾波2023/8/514（4）濾波增益方程Kk選取的原則2023/8/2115是用來估計的系數矩陣。

設狀態矢量和量測矢量都是一維的，且H=1，這說明增益系數是預測均方誤差在它和量測噪聲方差之和中所占的比例。一、卡爾曼濾波增益矩陣的直觀意義2023/8/515是用來估計2023/8/2116（5）一步預測均方誤差方程從下式可以看出，求Kk必須先求出Pk/k-1式中，為的估計誤差，可以看出一步預測均方誤差陣Pk/k-1是從估計均方誤差陣Pk-1轉移過來的，并且再加上系統噪聲方差的影響。的均方誤差陣，即：一、卡爾曼濾波2023/8/516（5）一步預測均方誤差方程從下式2023/8/2117二、自適應卡爾曼濾波在卡爾曼濾波計算中，可能出現發散的現象。當量測值數目不斷增加時，按濾波方程計算的估計均方誤差陣趨于零或趨于某一穩態值，但估計值和實際狀態的偏差卻越來越大，使濾波器逐漸失去估計作用。這種現象稱為濾波器的發散。2023/8/517二、自適應卡爾曼濾波在卡爾曼濾波2023/8/2118設系統和量測為：狀態和量測都是標量，和為互不相關的零均值白噪聲序列，并有：設，試求解狀態估計值，估計誤差方差以及真實誤差。在設計濾波器時誤認為系統沒有噪聲，即系統模型為：二、自適應卡爾曼濾波濾波發散例子2023/8/518設系統和量測為：狀態和量測2023/8/2119二、自適應卡爾曼濾波計算過程中的舍入誤差引起。系統的數學模型或噪聲模型的統計特性不準確，沒有能夠真實的反映系統的真實過程。濾波發散原因2023/8/519二、自適應卡爾曼濾波計算過程中的舍入誤差2023/8/2120二、自適應卡爾曼濾波Sage-Husa自適應濾波強跟蹤Kalman濾波改進的自適應濾波漸消記憶自適應濾波平方根濾波幾種主要的自適應濾波方法2023/8/520二、自適應卡爾曼濾波Sage-Husa自2023/8/2121二、自適應卡爾曼濾波

Sage-Husa自適應濾波利用觀測數據進行遞推濾波的同時，通過時變噪聲統計估值器，實時估計和修正系統噪聲和觀測噪聲的統計特性，從而達到降低模型誤差、抑制濾波發散、提高濾波精度的目的。2023/8/521二、自適應卡爾曼濾波 Sage-Husa2023/8/2122二、自適應卡爾曼濾波1、利用標準的卡爾曼濾波方程計算出各個狀態的估計值Sage-Husa自適應濾波2023/8/522二、自適應卡爾曼濾波1、利用標準的卡爾曼2023/8/2123二、自適應卡爾曼濾波2、計算系統噪聲和量測噪聲的均值和方差矩陣Sage-Husa自適應濾波2023/8/523二、自適應卡爾曼濾波2、計算系統噪聲和量2023/8/2124二、自適應卡爾曼濾波Sage-Husa自適應濾波實時解算系統噪聲統計特性，計算精度較高；增加了計算量，實時性難以保證；計算噪聲方差易失去正定性，穩定性和收斂性不能完全保證。2023/8/524二、自適應卡爾曼濾波Sage-Husa自2023/8/2125二、自適應卡爾曼濾波強跟蹤Kalman濾波通過犧牲一定的精度換取濾波穩定性將狀態估計一步預測均方誤差陣乘以加權系數2023/8/525二、自適應卡爾曼濾波強跟蹤Kalman濾2023/8/2126二、自適應卡爾曼濾波強跟蹤Kalman濾波狀態發生突變跟蹤狀態變化2023/8/526二、自適應卡爾曼濾波強跟蹤Kalman濾2023/8/2127二、自適應卡爾曼濾波

強跟蹤Kalman濾波結構簡單、跟蹤能力強、可靠性高；破壞了濾波器的最優條件，濾波精度降低。2023/8/527二、自適應卡爾曼濾波強跟蹤Kalman2023/8/2128二、自適應卡爾曼濾波改進的自適應濾波2023/8/528二、自適應卡爾曼濾波改進的自適應濾波2023/8/2129二、自適應卡爾曼濾波收斂性判據：儲備系數量測估計誤差，即新息改進的自適應濾波2023/8/529二、自適應卡爾曼濾波收斂性判據：儲備系數2023/8/2130二、自適應卡爾曼濾波卡爾曼濾波器發散，計算的協方差陣P逐漸趨于零，狀態估計過于依賴過去的數據，這種情況下，常采取漸消記憶的卡爾曼濾波來克服發散。采用漸消記憶限制Kalman濾波器對舊數據的利用權重，以便充分利用現時的觀測數據。漸消記憶自適應濾波2023/8/530二、自適應卡爾曼濾波卡爾曼2023/8/2131二、自適應卡爾曼濾波一步預測估計誤差方程漸消因子S>1，使增大，進而增大，使得新量測值在估計值中的權重增加，“過老”的量測值的作用相對地減少，從而抑制濾波發散。漸消記憶自適應濾波算法2023/8/531二、自適應卡爾曼濾波一步預測估計誤差方程2023/8/2132二、自適應卡爾曼濾波平方根濾波就是計算和的平方根L稱為A的平方根平方根濾波不但能保證和的非負定性，而且在數值計算中，計算的字長只須計算的字長的一半，就能達到相同的精度。平方根濾波算法2023/8/532二、自適應卡爾曼濾波平方根濾波就是計算33利用卡爾曼濾波技術對組合導航系統進行最優估計組合有兩種途徑：集中式濾波分散化濾波

利用一個卡爾曼濾波器來集中地處理所有導航子系統的信息三、聯邦卡爾曼濾波33利用卡爾曼濾波技術對組合導航系統進行最優估計組合有兩種途34濾波器子系統1參考系統時間更新最優融合子系統2子系統N三、聯邦卡爾曼濾波集中式濾波器結構34濾波器子系統1參考系統時間更新最優融合子系統2子系統N三35優點：(1)

可以給出最優的誤差估計；(2)

只有一個濾波器，結構簡單，工程中容易實現。缺點：(1)

狀態維數高，計算負擔重，帶來“維數災難”，不利于濾波實時運行；(2)

容錯性能差，不利于故障診斷，信息污染問題。三、聯邦卡爾曼濾波集中式濾波器特點35優點：(1)可以給出最優的誤差估計；(2)只有一個36利用卡爾曼濾波技術對組合導航系統進行最優估計組合的兩種途徑：分散化濾波

利用子濾波器處理子系統的信息通過全局濾波器實現所有子系統的信息融合集中式濾波三、聯邦卡爾曼濾波36利用卡爾曼濾波技術對組合導航系統進行最優估計組合的兩種途37聯邦濾波器一般結構LF2LF1LFN主濾波器子系統2子系統1子系統N參考系統時間更新最優融合三、聯邦卡爾曼濾波37聯邦濾波器一般結構LF2LF1LFN主濾波器子系統2子系38聯邦濾波要解決的問題：濾波器的容錯性能要好，濾波的精度要高；由局部濾波到全局濾波的融合算法要簡單，計算量小。

上述幾個性能要求是矛盾的。要容錯性能好，有時就要犧牲一些精度。為了解決這幾個性能要求，聯邦濾波中用了“誤差上界”技術和“信息分配”原則，獲得最佳折中的性能。三、聯邦卡爾曼濾波38聯邦濾波要解決的問題：濾波器的容錯性能要好，濾波的精度要2023/8/2139三、聯邦卡爾曼濾波若有N個局部狀態估計和相應的估計誤差協方差陣，且各局部估計互不相關，即，則全局最優估計可表示為：

若的估計精度差，即很大，則它在全局估計的貢獻就比較少。

各子濾波器的估計不相關時的融合算法其中2023/8/539三、聯邦卡爾曼濾波若有40信息分配原則首先說明什么是信息，系統中有兩類信息：（1）狀態運動方程的信息狀態方程的信息量是與系統噪聲的方差成反比的，因此可用Q-1表示狀態方程信息量；（2）量測方程的信息量測方程的信息可用量測噪聲協方差的逆R-1表示。三、聯邦卡爾曼濾波40信息分配原則首先說明什么是信息，系統中有兩類信息：（1）2023/8/2141三、聯邦卡爾曼濾波

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

信息分配原則2023/8/541三、聯邦卡爾曼濾波2023/8/2142三、聯邦卡爾曼濾波由子濾波器與主濾波器合成的全局估計值及其相應的估計誤差方差陣被放大為后再反饋到子濾波器，以重置子濾波器的估計值。

信息分配系數根據信息分配原則來確定，不同的值可獲得聯邦濾波器的不同結構和不同特性。

信息分配原則2023/8/542三、聯邦卡爾曼濾波由子濾2023/8/2143三、聯邦卡爾曼濾波聯邦濾波器的設計步驟：將子濾波器和主濾波器的初始估計誤差方差矩陣設置為組合導航系統初始估計誤差方差的倍，滿足信息守恒原則；將子濾波器和主濾波器的過程噪聲協方差矩陣設置為組合導航系統過程噪聲協方差矩陣的倍；各子濾波器處理自己的量測信息，獲得局部估計；在得到各子濾波器的局部估計和主濾波器的估計后，進行最優合成；用全局最優濾波解來重置各子濾波器和主濾波器的濾波值和估計誤差方差矩陣。

2023/8/543三、聯邦卡爾曼濾波聯邦濾波器的設計步驟：2023/8/2144三、聯邦卡爾曼濾波根據信息分配策略不同，聯邦濾波算法有4種實現模式：零復位模式變比例模式無反饋模式融合—反饋模式

聯邦濾波器結構2023/8/544三、聯邦卡爾曼濾波根據信息分配策略不同，45（1）零復位模式（零化式重置）LF2LF1LFN主濾波器子系統2子系統1子系統N參考系統時間更新最優融合三、聯邦卡爾曼濾波45（1）零復位模式（零化式重46優、缺點主濾波器分配到全部信息;故障檢測和隔離能力強;減少了數據通訊量，計算簡單，解算精度不高。（1）零復位模式（零化式重置）三、聯邦卡爾曼濾波46優、缺點（1）零復位模式（47（2）變比例模式（有重置）LF2LF1LFN主濾波器子系統2子系統1子系統N參考系統時間更新最優融合三、聯邦卡爾曼濾波47（2）變比例模式（48優、缺點主濾波器與子濾波器之間平均分配信息；融合后全局濾波精度高，局部濾波因為有全局濾波反饋，精度也提高了；一個子系統發生故障后，主濾波器受污染，隔離后必須重新初始化主濾波器。（2）變比例模式（有重置）三、聯邦卡爾曼濾波48優、缺點（2）變比例模式（49（3）無反饋模式（無重置）LF2LF1LFN主濾波器子系統2子系統1子系統N參考系統時間更新最優融合三、聯邦卡爾曼濾波2023/8/214949（3）無反饋