卡爾曼濾波 (英語:Kalman filter )是一種高效率的遞歸濾波器 (自我迴歸 濾波器),它能夠從一系列的不完全及包含雜訊 的測量 中,估計動態系統 的狀態。卡爾曼濾波會根據各測量量在不同時間下的值,考慮各時間下的聯合分佈 ,再產生對未知變數的估計,因此會比只以單一測量量為基礎的估計方式要準。卡爾曼濾波得名自主要貢獻者之一的魯道夫·卡爾曼 。
卡爾曼濾波器會追蹤系統的估計狀態,以及估計的變異量或是不確定性。會透過狀狀態轉換模型以及其量測來更新估計值。
x
^
k
∣
k
−
1
{\displaystyle {\hat {x}}_{k\mid k-1}}
是指時間k 時的估計,還沒有考慮第k 次量測y k 的資訊,
P
k
∣
k
−
1
{\displaystyle P_{k\mid k-1}}
為對應的不確定性
卡爾曼濾波在技術領域有許多的應用。常見的有飛機及太空船的導引、導航及控制 [ 1] 。卡爾曼濾波也廣為使用在時間序列 的分析中,例如訊號處理 及計量經濟學 中。卡爾曼濾波也是機械人 運動規劃及控制的重要主題之一,有時也包括在軌跡最佳化 。卡爾曼濾波也用在中軸神經系統運動控制的建模中。因為從給與運動命令到收到感覺神經的回授之間有時間差,使用卡爾曼濾波有助於建立符合實際的系統,估計運動系統的目前狀態,並且更新命令[ 2] 。
卡爾曼濾波的演算法是二步驟的程式。在估計步驟中,卡爾曼濾波會產生有關目前狀態的估計,其中也包括不確定性。只要觀察到下一個量測(其中一定含有某種程度的誤差,包括隨機雜訊)。會通過加權平均 來更新估計值,而確定性越高的量測加權比重也越高。演算法是迭代的,可以在實時控制系統 中執行,只需要目前的輸入量測、以往的計算值以及其不確定性矩陣,不需要其他以往的資訊。
使用卡爾曼濾波不用假設誤差是正態分佈 [ 3] ,不過若所有的誤差都是正態分佈,卡爾曼濾波可以得到正確的條件概率估計。
也發展了一些擴展或是廣義的卡爾曼濾波,例如運作在非線性系統的擴展卡爾曼濾波 及無跡卡爾曼濾波(英語:unscented Kalman filter )。底層的模型類似隱藏式馬可夫模型 ,不過潛在變數 的狀態空間是連續的,而且所有潛在變數及可觀測變數都是正態分佈。
卡爾曼濾波的一個典型實例是從一組有限的,包含噪聲的,通過對物體位置的觀察序列(可能有偏差)預測出物體的位置的坐標 及速度 。在很多工程應用(如雷達 、機器視覺 )中都可以找到它的身影。同時,卡爾曼濾波也是控制理論 以及控制系統工程 中的一個重要課題。
例如,對於雷達來說,人們感興趣的是其能夠跟蹤目標。但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態資訊,設法去掉噪聲的影響,得到一個關於目標位置的好的估計 。這個估計可以是對當前目標位置的估計(濾波),也可以是對於將來位置的估計(預測),也可以是對過去位置的估計(插值或平滑)。
這種濾波方法以它的發明者魯道夫·卡爾曼 (Rudolph Kalman)命名,但是根據文獻可知實際上Peter Swerling在更早之前就提出了一種類似的演算法。
斯坦利·施密特 (Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA艾姆斯研究中心 訪問時,發現他的方法對於解決阿波羅計劃 的軌道預測很有用,後來阿波羅飛船的導航電腦便使用了這種濾波器。關於這種濾波器的論文由Swerling(1958)、Kalman (1960)與Kalman and Bucy(1961)發表。
目前,卡爾曼濾波已經有很多不同的實現。卡爾曼最初提出的形式現在一般稱為簡單 卡爾曼濾波器。除此以外,還有施密特擴充 濾波器、資訊 濾波器以及很多Bierman, Thornton開發的平方根 濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環 ,它在收音機、電腦和幾乎任何影片或通訊裝置中廣泛存在。
以下的討論需要線性代數 以及概率論 的一般知識。
卡爾曼濾波器使用系統的動態模型(例如,運動的物理定律),該系統的已知控制輸入以及多個順序的測量值(例如來自感測器的測量值)來形成對系統變化量(其狀態)更好的估計,其精度比僅使用一種測量獲得的估算值高。它是一種常見的感測器融合 和數據融合 演算法。
感測器數據的雜訊,描述系統演化的方程式的近似值以及未考慮所有因素的外部因素都限制了確定系統狀態的能力。卡爾曼濾波器有效地處理了由於感測器數據雜訊引起的不確定性,並在一定程度上處理了隨機外部因素。卡爾曼濾波器使用加權平均值 生成系統狀態的估計值,作為系統預測狀態和新測量值的平均值。權重的目的是估計值具有更好(即較小)的不確定性的值會被更多「信任」。權重是根據協方差 來計算的,協方差是對系統狀態預測的估計不確定性的度量。加權平均值的結果是介於預測狀態和測量狀態之間的新狀態估計,並且比任何一個狀態都有更好的估計不確定性。在每個時間步重複此過程,新的估計值及其協方差將通知後續迭代中使用的預測。這意味着卡爾曼濾波器可以遞歸 地工作,並且只需要系統狀態的最後「最佳猜測」,而不是整個歷史,就可以計算新狀態。
測量和當前狀態估計的相對確定性是重要的考慮因素,通常根據卡爾曼濾波器的增益 來討論濾波器的反應。卡爾曼增益是賦予測量值和當前狀態估計值的相對權重,可以進行「調整」以獲得特定的效能。增益高時,濾波器將更多的精力放在最新的測量上,因此反應速度更快。增益較低時,濾波器會更緊密地遵循模型預測。在極端情況下,接近1的高增益將導致估計的軌跡更加跳躍,而接近零的低增益將消除雜訊,但會降低反應速度。
在執行濾波器的實際計算時(如下所述),狀態估計值和協方差被編碼到矩陣 中,以處理單個計算集中涉及的多個維度。這允許在任何過渡模型或協方差中表示不同狀態變數(例如位置,速度和加速度)之間的線性關係。
卡爾曼濾波建立在線性代數 和隱藏式馬可夫模型 (hidden Markov model)上。其基本動態系統可以用一個馬可夫鏈 表示,該馬可夫鏈建立在一個被高斯噪聲 (即正態分佈的噪聲)干擾的線性算子 上的。系統的狀態 可以用一個元素為實數的向量 表示。隨着離散時間 的每一個增加,這個線性算子 就會作用在當前狀態 上,產生一個新的狀態,並也會帶入一些噪聲,同時系統的一些已知的控制器的控制資訊也會被加入。同時,另一個受噪聲干擾的線性算子產生出這些隱含狀態的可見輸出。
為了從一系列有噪聲的觀察數據中用卡爾曼濾波器估計出被觀察過程的內部狀態,必須把這個過程在卡爾曼濾波的框架下建立模型。也就是說對於每一步k ,定義矩陣 F k , H k , Q k , R k ,有時也需要定義B k ,如下。
卡爾曼濾波器的模型。圓圈代表向量 ,方塊代表矩陣 ,星號代表高斯噪聲 ,其協方差矩陣 在右下方標出。
卡爾曼濾波模型假設k 時刻的真實狀態是從(k − 1)時刻的狀態演化而來,符合下式:
x
k
=
F
k
x
k
−
1
+
B
k
u
k
+
w
k
{\displaystyle {\textbf {x}}_{k}={\textbf {F}}_{k}{\textbf {x}}_{k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}+{\textbf {w}}_{k}}
其中
F k 是作用在x k −1 上的狀態轉換模型(/矩陣/向量)。
B k 是作用在控制器向量u k 上的輸入-控制模型。
w k 是過程噪聲,並假定其符合均值為零,協方差矩陣 為Q k 的多元正態分佈 。
w
k
∼
N
(
0
,
Q
k
)
{\displaystyle {\textbf {w}}_{k}\sim N(0,{\textbf {Q}}_{k})}
時刻k ,對真實狀態x k 的一個測量z k 滿足下式:
z
k
=
H
k
x
k
+
v
k
{\displaystyle {\textbf {z}}_{k}={\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}}
其中H k 是觀測模型,它把真實狀態空間對映成觀測空間,v k 是觀測噪聲,其均值為零,協方差矩陣為R k ,且服從正態分佈 。
v
k
∼
N
(
0
,
R
k
)
{\displaystyle {\textbf {v}}_{k}\sim N(0,{\textbf {R}}_{k})}
初始狀態以及每一時刻的噪聲{x 0 , w 1 , ..., w k , v 1 ... v k }都認為是互相獨立 的。
實際上,很多真實世界的動態系統都並不確切的符合這個模型;但是由於卡爾曼濾波器被設計在有噪聲的情況下工作,一個近似的符合已經可以使這個濾波器非常有用了。更多其它更複雜的卡爾曼濾波器的變種,在下邊討論中有描述。
卡爾曼濾波是一種遞歸 的估計,即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值,因此不需要記錄觀測或者估計的歷史資訊。卡爾曼濾波器與大多數濾波器不同之處,在於它是一種純粹的時域 濾波器,它不需要像低通濾波器 等頻域 濾波器那樣,需要在頻域設計再轉換到時域實現。
卡爾曼濾波器的狀態由以下兩個變數表示:
x
^
k
|
k
{\displaystyle {\hat {\textbf {x}}}_{k|k}}
,在時刻k 的狀態的估計;
P
k
|
k
{\displaystyle {\textbf {P}}_{k|k}}
,後驗估計誤差協方差矩陣,度量估計值的精確程度。
卡爾曼濾波器的操作包括兩個階段:預測 與更新 。在預測階段,濾波器使用上一狀態的估計,做出對當前狀態的估計。在更新階段,濾波器利用對當前狀態的觀測值最佳化在預測階段獲得的預測值,以獲得一個更精確的新估計值。
x
^
k
|
k
−
1
=
F
k
x
^
k
−
1
|
k
−
1
+
B
k
u
k
{\displaystyle {\hat {\textbf {x}}}_{k|k-1}={\textbf {F}}_{k}{\hat {\textbf {x}}}_{k-1|k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}}
(預測狀態)
P
k
|
k
−
1
=
F
k
P
k
−
1
|
k
−
1
F
k
T
+
Q
k
{\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}}
(預測估計協方差矩陣)
可參考:http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf (頁面存檔備份 ,存於互聯網檔案館 )
可參考:http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf (頁面存檔備份 ,存於互聯網檔案館 )
考慮在無摩擦的、無限長的直軌道上的一輛車。該車最初停在位置0處,但時不時受到隨機的衝擊。每隔Δt 秒即測量車的位置,但是這個測量是非精確的;想建立一個關於其位置以及速度 的模型。來看如何推導出這個模型以及如何從這個模型得到卡爾曼濾波器。
因為車上無動力,所以可以忽略掉B k 和u k 。由於F 、H 、R 和Q 是常數,所以時間下標可以去掉。
車的位置以及速度(或者更加一般的,一個粒子的運動狀態)可以被線性狀態空間描述如下:
x
k
=
[
x
x
˙
]
{\displaystyle {\textbf {x}}_{k}={\begin{bmatrix}x\\{\dot {x}}\end{bmatrix}}}
其中
x
˙
{\displaystyle {\dot {x}}}
是速度,也就是位置對於時間的導數。
假設在(k − 1)時刻與k 時刻之間,車受到a k 的加速度,其符合均值為0,標準差為σ a 的正態分佈 。根據牛頓運動定律 ,可以推出
x
k
=
F
x
k
−
1
+
G
a
k
{\displaystyle {\textbf {x}}_{k}={\textbf {F}}{\textbf {x}}_{k-1}+{\textbf {G}}a_{k}}
其中
F
=
[
1
Δ
t
0
1
]
{\displaystyle {\textbf {F}}={\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}}}
且
G
=
[
Δ
t
2
2
Δ
t
]
{\displaystyle {\textbf {G}}={\begin{bmatrix}{\frac {\Delta t^{2}}{2}}\\\Delta t\end{bmatrix}}}
可以發現
Q
=
cov
(
G
a
)
=
E
[
(
G
a
)
(
G
a
)
T
]
=
G
E
[
a
2
]
G
T
=
G
[
σ
a
2
]
G
T
=
σ
a
2
G
G
T
{\displaystyle {\textbf {Q}}={\textrm {cov}}({\textbf {G}}a)={\textrm {E}}[({\textbf {G}}a)({\textbf {G}}a)^{T}]={\textbf {G}}{\textrm {E}}[a^{2}]{\textbf {G}}^{T}={\textbf {G}}[\sigma _{a}^{2}]{\textbf {G}}^{T}=\sigma _{a}^{2}{\textbf {G}}{\textbf {G}}^{T}}
(因為σ a 是一個純量)。
在每一時刻,對其位置進行測量,測量受到噪聲干擾。假設噪聲服從正態分佈,均值為0,標準差為σ z 。
z
k
=
Hx
k
+
v
k
{\displaystyle {\textbf {z}}_{k}={\textbf {Hx}}_{k}+{\textbf {v}}_{k}}
其中
H
=
[
1
0
]
{\displaystyle {\textbf {H}}={\begin{bmatrix}1&0\end{bmatrix}}}
且
R
=
E
[
v
k
v
k
T
]
=
[
σ
z
2
]
{\displaystyle {\textbf {R}}={\textrm {E}}[{\textbf {v}}_{k}{\textbf {v}}_{k}^{T}]={\begin{bmatrix}\sigma _{z}^{2}\end{bmatrix}}}
如果知道足夠精確的車最初的位置,那麼可以初始化
x
^
0
|
0
=
[
0
0
]
{\displaystyle {\hat {\textbf {x}}}_{0|0}={\begin{bmatrix}0\\0\end{bmatrix}}}
並且,若讓濾波器知道確切的初始位置,可給出一個協方差矩陣:
P
0
|
0
=
[
0
0
0
0
]
{\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}}
如果不確切的知道最初的位置與速度,那麼協方差矩陣可以初始化為一個對角線元素是B 的矩陣,B 取一個合適的比較大的數。
P
0
|
0
=
[
B
0
0
B
]
{\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}B&0\\0&B\end{bmatrix}}}
此時,與使用模型中已有資訊相比,濾波器更傾向於使用初次測量值的資訊。
按照上邊的定義,從誤差協方差
P
k
|
k
{\displaystyle {\textbf {P}}_{k|k}}
開始推導如下:
P
k
|
k
=
cov
(
x
k
−
x
^
k
|
k
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k})}
代入
x
^
k
|
k
{\displaystyle {\hat {\textbf {x}}}_{k|k}}
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
y
~
k
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}))}
再代入
y
~
k
{\displaystyle {\tilde {\textbf {y}}}_{k}}
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
(
z
k
−
H
k
x
^
k
|
k
−
1
)
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))}
與
z
k
{\displaystyle {\textbf {z}}_{k}}
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
(
H
k
x
k
+
v
k
−
H
k
x
^
k
|
k
−
1
)
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))}
整理誤差向量,得
P
k
|
k
=
cov
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
|
k
−
1
)
−
K
k
v
k
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}((I-{\textbf {K}}_{k}{\textbf {H}}_{k})({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})-{\textbf {K}}_{k}{\textbf {v}}_{k})}
因為測量誤差v k 與其他項是非相關的,因此有
P
k
|
k
=
cov
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
∣
k
−
1
)
)
+
cov
(
K
k
v
k
)
{\displaystyle \mathbf {P} _{k|k}={\textrm {cov}}((I-\mathbf {K} _{k}\mathbf {H} _{k})(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}))+{\textrm {cov}}(\mathbf {K} _{k}\mathbf {v} _{k})}
利用協方差矩陣 的性質,此式可以寫作
P
k
|
k
=
(
I
−
K
k
H
k
)
cov
(
x
k
−
x
^
k
|
k
−
1
)
(
I
−
K
k
H
k
)
T
+
K
k
cov
(
v
k
)
K
k
T
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{T}+{\textbf {K}}_{k}{\textrm {cov}}({\textbf {v}}_{k}){\textbf {K}}_{k}^{T}}
使用不變數P k |k -1 以及R k 的定義這一項可以寫作
:
P
k
|
k
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
(
I
−
K
k
H
k
)
T
+
K
k
R
k
K
k
T
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{T}+{\textbf {K}}_{k}{\textbf {R}}_{k}{\textbf {K}}_{k}^{T}}
這一公式對於任何卡爾曼增益K k 都成立。如果K k 是最優卡爾曼增益,則可以進一步簡化,請見下文。
在卡爾曼增益等於上面匯出的最佳值時,計算後驗協方差的公式可以進行簡化。在卡爾曼增益公式兩側的右邊都乘以S k K k T 得到
K
k
S
k
K
k
T
=
P
k
|
k
−
1
H
k
T
K
k
T
{\displaystyle {\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}}
根據上面後驗誤差協方差展開公式,
P
k
|
k
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
−
P
k
|
k
−
1
H
k
T
K
k
T
+
K
k
S
k
K
k
T
{\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}+{\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}}
最後兩項可以抵消,得到
P
k
|
k
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
{\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}}
.
這個公式的計算比較簡單,所以實際中總是使用這個公式,但是需注意這公式僅在使用最佳卡爾曼增益的時候它才成立。如果算術精度總是很低而導致數值穩定性 出現問題,或者特意使用非最佳卡爾曼增益,那麼就不能使用這個簡化;必須使用上面匯出的後驗誤差協方差公式。
假設真正的狀態是無法觀察的馬可夫過程 ,測量結果是從隱性馬可夫模型觀察到的狀態。
根據馬可夫假設,真正的狀態僅受最近一個狀態影響而與其它以前狀態無關。
p
(
x
k
|
x
0
,
…
,
x
k
−
1
)
=
p
(
x
k
|
x
k
−
1
)
{\displaystyle p({\textbf {x}}_{k}|{\textbf {x}}_{0},\dots ,{\textbf {x}}_{k-1})=p({\textbf {x}}_{k}|{\textbf {x}}_{k-1})}
與此類似,在時刻k 測量只與當前狀態有關而與其它狀態無關。
p
(
z
k
|
x
0
,
…
,
x
k
)
=
p
(
z
k
|
x
k
)
{\displaystyle p({\textbf {z}}_{k}|{\textbf {x}}_{0},\dots ,{\textbf {x}}_{k})=p({\textbf {z}}_{k}|{\textbf {x}}_{k})}
根據這些假設,隱性馬可夫模型所有狀態的概率分佈可以簡化為:
p
(
x
0
,
…
,
x
k
,
z
1
,
…
,
z
k
)
=
p
(
x
0
)
∏
i
=
1
k
p
(
z
i
|
x
i
)
p
(
x
i
|
x
i
−
1
)
{\displaystyle p({\textbf {x}}_{0},\dots ,{\textbf {x}}_{k},{\textbf {z}}_{1},\dots ,{\textbf {z}}_{k})=p({\textbf {x}}_{0})\prod _{i=1}^{k}p({\textbf {z}}_{i}|{\textbf {x}}_{i})p({\textbf {x}}_{i}|{\textbf {x}}_{i-1})}
然而,當卡爾曼濾波器用來估計狀態x 時,感興趣的概率分佈,是基於目前為止所有個測量值來得到的當前狀態之概率分佈
p
(
x
k
|
Z
k
−
1
)
=
∫
p
(
x
k
|
x
k
−
1
)
p
(
x
k
−
1
|
Z
k
−
1
)
d
x
k
−
1
{\displaystyle p({\textbf {x}}_{k}|{\textbf {Z}}_{k-1})=\int p({\textbf {x}}_{k}|{\textbf {x}}_{k-1})p({\textbf {x}}_{k-1}|{\textbf {Z}}_{k-1})\,d{\textbf {x}}_{k-1}}
在資訊濾波器或逆協方差濾波器中,估計的協方差和估計狀態分別由資訊矩陣 和資訊 向量代替。 這些定義為:
Y
k
∣
k
=
P
k
∣
k
−
1
y
^
k
∣
k
=
P
k
∣
k
−
1
x
^
k
∣
k
{\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}{\hat {\mathbf {x} }}_{k\mid k}\end{aligned}}}
同樣,預測的協方差和狀態具有等效的資訊形式,定義為:
Y
k
∣
k
−
1
=
P
k
∣
k
−
1
−
1
y
^
k
∣
k
−
1
=
P
k
∣
k
−
1
−
1
x
^
k
∣
k
−
1
{\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}{\hat {\mathbf {x} }}_{k\mid k-1}\end{aligned}}}
以及測量協方差和測量向量,它們定義為:
I
k
=
H
k
T
R
k
−
1
H
k
i
k
=
H
k
T
R
k
−
1
z
k
{\displaystyle {\begin{aligned}\mathbf {I} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {H} _{k}\\\mathbf {i} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {z} _{k}\end{aligned}}}
資訊更新現在變得微不足道了。
Y
k
∣
k
=
Y
k
∣
k
−
1
+
I
k
y
^
k
∣
k
=
y
^
k
∣
k
−
1
+
i
k
{\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\mathbf {I} _{k}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\mathbf {i} _{k}\end{aligned}}}
資訊過濾器的主要優點是,只需將其測量資訊矩陣和向量相加即可在每個時間步長過濾N個測量值。
Y
k
∣
k
=
Y
k
∣
k
−
1
+
∑
j
=
1
N
I
k
,
j
y
^
k
∣
k
=
y
^
k
∣
k
−
1
+
∑
j
=
1
N
i
k
,
j
{\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\sum _{j=1}^{N}\mathbf {I} _{k,j}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\sum _{j=1}^{N}\mathbf {i} _{k,j}\end{aligned}}}
為了預測資訊過濾器,可以將資訊矩陣和向量轉換回它們的狀態空間等效項,或者可以使用資訊空間預測。
M
k
=
[
F
k
−
1
]
T
Y
k
−
1
∣
k
−
1
F
k
−
1
C
k
=
M
k
[
M
k
+
Q
k
−
1
]
−
1
L
k
=
I
−
C
k
Y
k
∣
k
−
1
=
L
k
M
k
L
k
T
+
C
k
Q
k
−
1
C
k
T
y
^
k
∣
k
−
1
=
L
k
[
F
k
−
1
]
T
y
^
k
−
1
∣
k
−
1
{\displaystyle {\begin{aligned}\mathbf {M} _{k}&=\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}\mathbf {Y} _{k-1\mid k-1}\mathbf {F} _{k}^{-1}\\\mathbf {C} _{k}&=\mathbf {M} _{k}\left[\mathbf {M} _{k}+\mathbf {Q} _{k}^{-1}\right]^{-1}\\\mathbf {L} _{k}&=\mathbf {I} -\mathbf {C} _{k}\\\mathbf {Y} _{k\mid k-1}&=\mathbf {L} _{k}\mathbf {M} _{k}\mathbf {L} _{k}^{\textsf {T}}+\mathbf {C} _{k}\mathbf {Q} _{k}^{-1}\mathbf {C} _{k}^{\textsf {T}}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {L} _{k}\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}{\hat {\mathbf {y} }}_{k-1\mid k-1}\end{aligned}}}
如果F和Q是非時變的,則可以將這些值快取起來,並且F和Q必須是可逆的。
基本卡爾曼濾波器(The basic Kalman filter)是限制在線性的假設之下。然而,大部份非平凡的(non-trivial)的系統都是非線性系統。其中的「非線性性質」(non-linearity)可能是伴隨存在過程模型(process model)中或觀測模型(observation model)中,或者兩者兼有之。
在擴充卡爾曼濾波器(Extended Kalman Filter,簡稱EKF)中狀態轉換和觀測模型不需要是狀態的線性函數,可替換為(可微的)函數。
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
{\displaystyle {\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},{\textbf {w}}_{k})}
z
k
=
h
(
x
k
,
v
k
)
{\displaystyle {\textbf {z}}_{k}=h({\textbf {x}}_{k},{\textbf {v}}_{k})}
函數f 可以用來從過去的估計值中計算預測的狀態,相似的,函數h 可以用來以預測的狀態計算預測的測量值。然而f 和h 不能直接的應用在協方差中,取而代之的是計算偏導矩陣(Jacobian )。
在每一步中使用當前的估計狀態計算Jacobian矩陣,這幾個矩陣可以用在卡爾曼濾波器的方程中。這個過程,實質上將非線性的函數在當前估計值處線性化了。
這樣一來,卡爾曼濾波器的等式為:
預測
x
^
k
|
k
−
1
=
f
(
x
k
−
1
,
u
k
,
0
)
{\displaystyle {\hat {\textbf {x}}}_{k|k-1}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},0)}
P
k
|
k
−
1
=
F
k
P
k
−
1
|
k
−
1
F
k
T
+
Q
k
{\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}}
使用Jacobians矩陣更新模型
F
k
=
∂
f
∂
x
|
x
^
k
−
1
|
k
−
1
,
u
k
{\displaystyle {\textbf {F}}_{k}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k}}}
H
k
=
∂
h
∂
x
|
x
^
k
|
k
−
1
{\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}
更新
y
~
k
=
z
k
−
h
(
x
^
k
|
k
−
1
,
0
)
{\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-h({\hat {\textbf {x}}}_{k|k-1},0)}
S
k
=
H
k
P
k
|
k
−
1
H
k
T
+
R
k
{\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}}
K
k
=
P
k
|
k
−
1
H
k
T
S
k
−
1
{\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}}
x
^
k
|
k
=
x
^
k
|
k
−
1
+
K
k
y
~
k
{\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}}
P
k
|
k
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}}
預測
如同擴展卡爾曼濾波器(EKF)一樣, UKF的預測過程可以獨立於UKF的更新過程之外,與一個線性的(或者確實是擴展卡爾曼濾波器的)更新過程合併來使用;或者,UKF的預測過程與更新過程在上述中地位互換亦可。
卡爾曼-布西濾波器(Kalman-Bucy filter,以Richard Snowden Bucy命名)是Kalman濾波器的連續時間版本。
它基於狀態空間模型
d
d
t
x
(
t
)
=
F
(
t
)
x
(
t
)
+
B
(
t
)
u
(
t
)
+
w
(
t
)
z
(
t
)
=
H
(
t
)
x
(
t
)
+
v
(
t
)
{\displaystyle {\begin{aligned}{\frac {d}{dt}}\mathbf {x} (t)&=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t)\\\mathbf {z} (t)&=\mathbf {H} (t)\mathbf {x} (t)+\mathbf {v} (t)\end{aligned}}}
其中
Q
(
t
)
{\displaystyle \mathbf {Q} (t)}
和
R
(
t
)
{\displaystyle \mathbf {R} (t)}
分別代表兩個白噪聲項
w
(
t
)
{\displaystyle \mathbf {w} (t)}
和
v
(
t
)
{\displaystyle \mathbf {v} (t)}
的強度(或更準確地說:功率譜密度-PSD-矩陣)。
該濾波器由兩個微分方程組成,一個用於狀態估計,一個用於協方差:
d
d
t
x
^
(
t
)
=
F
(
t
)
x
^
(
t
)
+
B
(
t
)
u
(
t
)
+
K
(
t
)
(
z
(
t
)
−
H
(
t
)
x
^
(
t
)
)
d
d
t
P
(
t
)
=
F
(
t
)
P
(
t
)
+
P
(
t
)
F
T
(
t
)
+
Q
(
t
)
−
K
(
t
)
R
(
t
)
K
T
(
t
)
{\displaystyle {\begin{aligned}{\frac {d}{dt}}{\hat {\mathbf {x} }}(t)&=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {K} (t)\left(\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t)\right)\\{\frac {d}{dt}}\mathbf {P} (t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} ^{\textsf {T}}(t)+\mathbf {Q} (t)-\mathbf {K} (t)\mathbf {R} (t)\mathbf {K} ^{\textsf {T}}(t)\end{aligned}}}
卡爾曼增益由
K
(
t
)
=
P
(
t
)
H
T
(
t
)
R
−
1
(
t
)
{\displaystyle \mathbf {K} (t)=\mathbf {P} (t)\mathbf {H} ^{\textsf {T}}(t)\mathbf {R} ^{-1}(t)}
注意,在此表達式中,對於
K
(
t
)
{\displaystyle \mathbf {K} (t)}
,觀察雜訊
R
(
t
)
{\displaystyle \mathbf {R} (t)}
的協方差同時表示預測誤差(或創新)
y
~
(
t
)
=
z
(
t
)
−
H
(
t
)
x
^
(
t
)
{\displaystyle {\tilde {\mathbf {y} }}(t)=\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t)}
的協方差; 這些協方差僅在連續時間的情況下才相等。
離散時間卡爾曼濾波的預測步驟和更新步驟之間的區別在連續時間內不存在。
用於協方差的第二個微分方程是Riccati方程 的一個範例。
大多數物理系統表示為連續時間模型,而離散時間測量則經常通過數字處理器進行狀態估計。 因此,系統模型和測量模型由下式給出:
x
˙
(
t
)
=
F
(
t
)
x
(
t
)
+
B
(
t
)
u
(
t
)
+
w
(
t
)
,
w
(
t
)
∼
N
(
0
,
Q
(
t
)
)
z
k
=
H
k
x
k
+
v
k
,
v
k
∼
N
(
0
,
R
k
)
{\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t),&\mathbf {w} (t)&\sim N\left(\mathbf {0} ,\mathbf {Q} (t)\right)\\\mathbf {z} _{k}&=\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k},&\mathbf {v} _{k}&\sim N(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}}
當
x
k
=
x
(
t
k
)
{\displaystyle \mathbf {x} _{k}=\mathbf {x} (t_{k})}
.
x
^
0
∣
0
=
E
[
x
(
t
0
)
]
,
P
0
∣
0
=
Var
[
x
(
t
0
)
]
{\displaystyle {\hat {\mathbf {x} }}_{0\mid 0}=E\left[\mathbf {x} (t_{0})\right],\mathbf {P} _{0\mid 0}=\operatorname {Var} \left[\mathbf {x} \left(t_{0}\right)\right]}
x
^
˙
(
t
)
=
F
(
t
)
x
^
(
t
)
+
B
(
t
)
u
(
t
)
, with
x
^
(
t
k
−
1
)
=
x
^
k
−
1
∣
k
−
1
⇒
x
^
k
∣
k
−
1
=
x
^
(
t
k
)
P
˙
(
t
)
=
F
(
t
)
P
(
t
)
+
P
(
t
)
F
(
t
)
T
+
Q
(
t
)
, with
P
(
t
k
−
1
)
=
P
k
−
1
∣
k
−
1
⇒
P
k
∣
k
−
1
=
P
(
t
k
)
{\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t){\text{, with }}{\hat {\mathbf {x} }}\left(t_{k-1}\right)={\hat {\mathbf {x} }}_{k-1\mid k-1}\\\Rightarrow {\hat {\mathbf {x} }}_{k\mid k-1}&={\hat {\mathbf {x} }}\left(t_{k}\right)\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\textsf {T}}+\mathbf {Q} (t){\text{, with }}\mathbf {P} \left(t_{k-1}\right)=\mathbf {P} _{k-1\mid k-1}\\\Rightarrow \mathbf {P} _{k\mid k-1}&=\mathbf {P} \left(t_{k}\right)\end{aligned}}}
預測方程式是從連續時間卡爾曼濾波器的方程式推導而得,而無需進行測量更新,例如:
K
(
t
)
=
0
{\displaystyle \mathbf {K} (t)=0}
。預測狀態和協方差分別通過求解一組初始值等於上一步估計值的微分方程組來計算。
在線性非時變系統 的情況下,可以使用矩陣指數 將連續時間動態精確地離散化 為離散時間系統。
K
k
=
P
k
∣
k
−
1
H
k
T
(
H
k
P
k
∣
k
−
1
H
k
T
+
R
k
)
−
1
x
^
k
∣
k
=
x
^
k
∣
k
−
1
+
K
k
(
z
k
−
H
k
x
^
k
∣
k
−
1
)
P
k
∣
k
=
(
I
−
K
k
H
k
)
P
k
∣
k
−
1
{\displaystyle {\begin{aligned}\mathbf {K} _{k}&=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\left(\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}+\mathbf {R} _{k}\right)^{-1}\\{\hat {\mathbf {x} }}_{k\mid k}&={\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}\left(\mathbf {z} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1}\right)\\\mathbf {P} _{k\mid k}&=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k\mid k-1}\end{aligned}}}
更新方程與離散時間卡爾曼濾波器的更新方程相同。
Gelb A., editor. Applied optimal estimation. MIT Press, 1974. Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960) Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961) [JU97] Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997. Harvey, A.C. Forecasting, Structural Time Series Models and the Kalman Filter. Cambridge University Press, Cambridge, 1989.
Wolpert, Daniel; Ghahramani, Zoubin. Computational principles of movement neuroscience. Nature Neuroscience. 2000, 3 : 1212–7. PMID 11127840 . doi:10.1038/81497 .