濾波器模型的建立
卡爾曼濾波器包括兩個主要過程：預估與校正。預估過程
主要是利用時間更新方程建立對當前狀態的先驗估計，及時向
前推算當前狀態變量和誤差協方差估計的值，以便為下一個時
間狀態構造先驗估計值；校正過程負責反饋，利用測量更新方
程在預估過程的先驗估計值及當前測量變量的基礎上建立起
對當前狀態的改進的後驗估計。這樣的一個過程，我們稱之為
預估－校正過程，對應的這種估計算法稱為預估－校正算法。以
下給出離散卡爾曼濾波的時間更新方程和狀態更新方程。
時間更新方程：
X贊
k
－＝AX贊 k-1＋BU贊 k-1 （11）
Pk
－＝APk-1AT＋Q （12）
狀態更新方程：
Kk＝Pk
－HT（HPk
－HT＋R）－2 （13）
X贊
k＝X贊 k
－＋Kk（Zk－HX贊 k
－） （14）
Pk＝（I－KkH）Pk
－ （15）
在上面式中，各量説明如下：
A：作用在Xk-1
上的n×n 狀態變換矩陣
B：作用在控制向量Uk-1
上的n×1 輸入控制矩陣
H：m×n 觀測模型矩陣， 它把真實狀態空間映射成觀測空
間Pk
－：為n×n 先驗估計誤差協方差矩陣Pk：為n×n 後驗估計誤
差協方差矩陣Q：n×n 過程噪聲協方差矩陣R：m×m 過程噪聲
協方差矩陣I：n×n 階單位矩陣Kk：n×m 階矩陣， 稱為卡爾曼增
益或混合因數，作用是使後驗估計誤差協方差最小前面描述的
卡爾曼濾波器估計一個用線性隨機差分方程描述的隨機過程
的狀態變量Xk∈Rn