卡尔曼滤波广泛应用于智能导航与跟踪算法中,本文根据参考paper详解卡尔曼滤波原理,并结合matlab源码更好地理解paper
前期概念:
信号可以如下表示:
y
k
y_k
yk是观测信号,
a
k
a_k
ak是增益,
x
k
x_k
xk是信息,
n
k
n_k
nk是noise
目标是估计
x
k
x_k
xk,
估计值
x
^
k
\hat x_k
x^k和真实值
x
k
x_k
xk之间的误差为
如果用平方误差,则
估计一段时间的误差则是求期望值
用最大似然推导,即是最大化估计x时y的概率:
假设加性噪声是高斯分布,则:
K
k
K_k
Kk是归一化常数,则最大似然函数(期望值)为
取log
卡尔曼滤波推导:
state空间
在过程中的状态:
x
k
x_k
xk是k时刻的状态向量,
Φ
\Phi
Φ是k时刻状态到k+1时刻状态的状态转移矩阵,是不随时间变化的,
w
k
w_k
wk是方差已知的噪声。
观测值
z
k
z_k
zk是k时刻
x
k
x_k
xk的实际观测值,H是状态向量到measurement向量的连接矩阵,不随时间变化,
v
k
v_k
vk是measurement error,不随时间变化,也是方差已知的白噪声。
前面最大似然处的推导可知,用MSE估计最优filter的前提是error必须是高斯分布。两个噪声的协方差用以下变量表示
假设
x
^
k
′
\hat x_k^\prime
x^k′是
x
^
k
\hat x_k
x^k的前一个估计,
K
k
K_k
Kk是卡尔曼增益,那么
所以
k时刻的误差协方差矩阵为
状态映射为下式
所以误差
因此
上式中
e
k
e_k
ek和
w
k
w_k
wk是不相关的,因为噪声
w
k
w_k
wk只是在k时刻和k+1时刻之间的噪声,而误差
e
k
e_k
ek则是从开始积累到k时刻的误差,因此
代码中上面的 Φ \Phi Φ用矩阵A表示
最终卡尔曼滤波的两步,predict和correct分别做如下的工作
具体怎么用,结合matlab代码中的predict和correct来说明
matlab代码可参照示例
示例为小球的跟踪
跟踪首先要有目标的初始位置,这个位置可以是人为规定,也可以是检测算法检测出来。该示例中采用检测算法得到初始位置,correct函数中也是用每帧检测到的位置来更新卡尔曼滤波的参数。colorImage为读入的每帧图像
第一帧的时候配置卡尔曼滤波的初始化参数
进入configureKalmanFilter函数,可看到配置了如下参数
其中Qs为[25, 10, 10]
如果已经不是初始位置了,之后的步骤如下:
如果判断检测的位置准确,可以用检测的位置来更新卡尔曼滤波的参数
具体步骤如下:
(1) predict函数得到
x
^
k
−
\hat x_k^-
x^k−和
P
k
−
P_k^-
Pk−
实际观测值为
z
k
z_k
zk,即检测到的位置
(2) correct函数更新参数
对应的代码中可看到,当检测到目标时,用检测的位置更新参数(predict + correct),没有检测到目标时,就仅更新
x
^
k
−
\hat x_k^-
x^k−和
P
k
−
P_k^-
Pk−(predict)