第三章 卡爾曼濾波3.2 算法和模型-2卡爾曼濾波算法


離散時間卡爾曼濾波算法包含以下步驟:

 不必嚴格遵守這個順序,

前四個步驟組成了卡爾曼濾波的系統傳播流程,也被稱為是系統更新、系統外推、預測、時間更新或者時間傳播流程。

狀態轉移矩陣定義了狀態向量隨時間的變化規律,在卡爾曼濾波系統模型中,狀態是系統動力學過程的函數。

狀態轉移矩陣必然是卡爾曼濾波迭代時間間隔的函數,當這些參數隨時間發生變化時,就必須在每次迭代中重新計算狀態轉移矩陣。

步驟(2)計算系統噪聲協方差矩陣(system noise covariance matrix)Qk-1,也被稱為過程噪聲協方差矩陣。

系統噪聲常常是迭代時間間隔的函數,針對不同的應用,可將其建模為時變值或常值(對給定的時間間隔)。

步驟(3)狀態向量估計值隨時間的傳播,采用下式:

 步驟(4)對應的誤差協方差矩陣的傳播,標准形式為

 

 步驟(5)計算觀測矩陣(measurement matrix)Hk,該矩陣定義了觀測向量隨狀態的變化規律,其中行對應觀測值,列對應狀態。

在大多數應用中,觀測矩陣是變化的,所以在每次卡爾曼濾波迭代中都必須重新計算。在導航中,Hk常常是載體運動學或GNSS衛星幾何構型的函數。

步驟(6)計算觀測噪聲協方差矩陣Rk。在不同應用中,可假設為常量,也可建模為動力學和或信噪比測量的函數。

步驟(7)計算卡爾曼增益矩陣(卡爾曼gain matrix)Kk。該矩陣決定了觀測信息在狀態更新時的權值。行對應狀態,列對應觀測值。卡爾曼增益取決於實際測量向量zk的誤差協方差矩陣,和狀態估計預測的觀測向量的誤差協方差矩陣。

誤差協方差矩陣的對角元素為不確定度的平方。

由式 (3.8) -式 (3.10) 知,真實觀測向量的誤差協方差為

 從式(3.5)可得到由狀態向量預測觀測向量誤差協方差

 卡爾曼增益矩陣為

 注意,由於式(3.20)中第一項Hk矩陣在方差比的分子項中被去掉,卡爾曼增益矩陣以及對觀測信息進行的加權,都從觀測空間轉移到了狀態空間。矩陣Pk-非對角元素中的相關信息,把觀測向量和哪些不能直接通過Hk矩陣與觀測相關的狀態耦合到了一起。

步驟(8)觀測向量zk的構建。

步驟(9)用觀測向量更新狀態向量,公式為

 步驟(10)對應的誤差協方差矩陣更新,采用以下公式:

 更新后的狀態向量估計,建立於更多信息的基礎上,更新后狀態的不確定度要比更新之前小。

 這里介紹的算法為卡爾曼濾波的開環形式,所有的狀態估計都保留在卡爾曼濾波算法內。閉環形式里,狀態估計被反饋到系統里用以校正系統參數。

 


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM