卡尔曼滤波器:预测和观测的融合
整体理解
卡尔曼滤波器是一种高效的自回归滤波器。具有较强的鲁棒性。其鲁棒性来源于对不确定度的有效建模。对于卡尔曼滤波器,假定系统的真实状态为X,由于现实中存在种种不确定性,我们无法确定真实状态的数值,只能给出他的分布,即真实状态在状态空间的各个位置出现的概率。
基于前一个状态和转移模型,我们可以给出对当前状态的预测。
基于传感器等对当前状态的量测,我们可以给出对当前状态的观测。
预测和观测都是高斯分布,卡尔曼滤波器将这两个高斯函数“相乘”,得到当前状态。
Presumption1 假定系统状态X服从高斯分布,那么系统可以由均值和方差唯一确定。
图中,均值为
μ
0
\mu_0
μ0的高斯函数和均值为
μ
1
\mu_1
μ1的高斯函数相乘,得到均值和方差分别为
(
μ
′
,
σ
′
)
(\mu',\sigma')
(μ′,σ′)的高斯函数大致如图。
上图为一维高斯,当变量为多维时,均值变为均值向量;高斯分布的各个维度可能相互影响,方差变为协方差矩阵
Σ
i
j
\Sigma_{ij}
Σij.
系统状态
系统当前的状态为
x
k
x_k
xk(严格来说,是状态的均值为
x
k
x_k
xk,或者,最有可能的状态为
x
k
x_k
xk),方差为
P
k
P_k
Pk.
举个例子,当系统处于匀速运动时,系统的状态可以由 {位置; 速度} 两个量描述。
STEP1 预测
Presumption2 假定系统的转移模式T是确定的,已知系统的当前状态,则系统下一时刻的状态可以唯一确定。
F k F_k Fk为系统的运动转移方程。
已知前一时刻状态的分布和转移矩阵,要求解当前时刻的状态。
上面的推导比较理想,现在加上人为制动和环境扰动的考虑,当前时刻的状态更改如下,得到最终的预测状态。
系统的探测器如传感器等传递的观测数据
z
k
z_k
zk可能是目标离探测器的距离等,和系统的状态如位置、速度等并不直接等价,往往需要转换矩阵
H
k
H_k
Hk。将对系统的预测状态,转化为对系统的预测观测。
Z
预测
=
H
k
⋅
X
k
Z_{预测} = H_k\cdot X_k
Z预测=Hk⋅Xk
STEP2 观测
探测器往往也带有误差。基于探测器提供的数据,得到的观测状态为
传感器噪声产生的协方差矩阵
R
k
R_k
Rk
传感器分布均值
Z
k
Z_k
Zk
融合预测和观测
预测和观测步分别得到了一个高斯分布,将这两个高斯分布相乘,即得到了系统的当前状态。
参考文献
【1】https://zhuanlan.zhihu.com/p/39912633