離散時間卡爾曼濾波算法包含以下步驟:
不必嚴格遵守這個順序,
前四個步驟組成了卡爾曼濾波的系統傳播流程,也被稱為是系統更新、系統外推、預測、時間更新或者時間傳播流程。
狀態轉移矩陣定義了狀態向量隨時間的變化規律,在卡爾曼濾波系統模型中,狀態是系統動力學過程的函數。
狀態轉移矩陣必然是卡爾曼濾波迭代時間間隔的函數,當這些參數隨時間發生變化時,就必須在每次迭代中重新計算狀態轉移矩陣。
步驟(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)對應的誤差協方差矩陣更新,采用以下公式:
更新后的狀態向量估計,建立於更多信息的基礎上,更新后狀態的不確定度要比更新之前小。
這里介紹的算法為卡爾曼濾波的開環形式,所有的狀態估計都保留在卡爾曼濾波算法內。閉環形式里,狀態估計被反饋到系統里用以校正系統參數。