系統的狀态方程為:
這個狀态方程是根據上一時刻的狀态和控制變量來推測此刻的狀态,wk-1是服從高斯分布的噪聲,是預測過程的噪聲,它對應了 xk 中每個分量的噪聲,是期望為 0,協方差為 Q 的高斯白噪聲wk-1~N(0,Q),Q即下文的過程激勵噪聲Q.
觀測方程為:
vk是觀測的噪聲,服從高斯分布,vk~N(0,R),R即下文的測量噪聲R。
卡爾曼濾波算法有兩個基本假設: ( 1) 資訊過程的足夠精确的模型,是由白噪聲所激發的線性( 也可以是時變的) 動态系統; ( 2) 每次的測量信号都包含着附加的白噪聲分量 。當滿足以上假設時,可以應用卡爾曼濾波算法。
卡爾曼濾波算法分為兩步:預測和更新
預測:根據上一時刻( k - 1 時刻) 的後驗估計值來估計目前時刻( k 時刻) 的狀态,得到 k 時刻的先驗估計值;
更新:使用目前時刻的測量值來更正預測階段估計值,得到目前時刻的後驗估計值。
卡爾曼濾波器可以分為時間更新方程和測量更新方程。時間更新方程(即預測階段)根據前一時刻的狀态估計值推算目前時刻的狀态變量先驗估計值和誤差協方差先驗估計值; 測量更新方程(即更新階段)負責将先驗估計和新的測量變量結合起來構造改進的後驗估計。時間更新方程和測量更新方程也被稱為預測方程和校正方程。是以卡爾曼算法是一個遞歸的預測—校正方法。
五大公式:
下面來一個個詳細剖析每個參數:
1,
: 分别表示 k - 1 時刻和 k 時刻的後驗狀态估計值,是濾波的結果之一,即更新後的結果,也叫最優估計(估計的狀态,根據理論,我們不可能知道每時刻狀态的确切結果是以叫估計)。
2,
: k 時刻的先驗狀态估計值,是濾波的中間計算結果,即根據上一時刻(k-1時刻)的最優估計預測的k時刻的結果,是預測方程的結果。
3,
:分别表示 k - 1 時刻和 k 時刻的後驗估計協方差(即
的協方差,表示狀态的不确定度),是濾波的結果之一。
4,
:k 時刻的先驗估計協方差(
的協方差),是濾波的中間計算結果。
5,
:是狀态變量到測量(觀測)的轉換矩陣,表示将狀态和觀測連接配接起來的關系,卡爾曼濾波裡為線性關系,它負責将 m 維的測量值轉換到 n 維,使之符合狀态變量的數學形式,是濾波的前提條件之一。
6,
:測量值(觀測值),是濾波的輸入。
7,
:濾波增益矩陣,是濾波的中間計算結果,卡爾曼增益,或卡爾曼系數。
8,
:狀态轉移矩陣,實際上是對目标狀态轉換的一種猜想模型。例如在機動目标跟蹤中, 狀态轉移矩陣常常用來對目标的運動模組化,其模型可能為勻速直線運動或者勻加速運動。當狀态轉移矩陣不符合目标的狀态轉換模型時,濾波會很快發散。
9,Q:過程激勵噪聲協方差(系統過程的協方差)。該參數被用來表示狀态轉換矩陣與實際過程之間的誤差。因為我們無法直接觀測到過程信号, 是以 Q 的取值是很難确定的。是卡爾曼濾波器用于估計離散時間過程的狀态變量,也叫預測模型本身帶來的噪聲。狀态轉移協方差矩陣
10:R: 測量噪聲協方差。濾波器實際實作時,測量噪聲協方差 R一般可以觀測得到,是濾波器的已知條件。
11,B:是将輸入轉換為狀态的矩陣
12,
:實際觀測和預測觀測的殘差,和卡爾曼增益一起修正先驗(預測),得到後驗。