卡尔曼滤波 (英語: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 .