MATLAB自动驾驶工具箱——卡尔曼运动跟踪示例解读

引言

自动驾驶感知中，需要对物体（主要是车辆）进行实时跟踪与预测，是车辆进行决策的前提条件之一。MATLAB自动驾驶工具箱rackingKF类及相关函数提供了线性卡尔曼滤波器可以在位置、速度和加速度上对物体的运动进行跟踪。本文将基于线性卡尔曼滤波的理论知识对MATLAB帮助文档中的rackingKF类实例进行解读。

trackingKF 简介

trackingKF 可以对车辆、行人、自行车和静止物体或障碍物的跟踪，它假设卡尔曼滤波器的过程噪声和测量噪声(process and measurements)都是加性噪声(additive noise)，当以上噪声都是高斯噪声时，卡尔曼滤波器的优化目标就是线性过程的最小二乘估计。

trackingKF有两种使用方法：

1. 直接设置运动类型。
2. 直接使用预定义好的运动模型。

运动模型
‘1D Constant Velocity’
‘1D Constant Acceleration’
‘2D Constant Velocity’
‘2D Constant Acceleration’
‘3D Constant Velocity’
‘3D Constant Acceleration’

预定义好的模型包括了恒速模型、恒加速度以及x、y、z三个方向共6种模型。

trackingKF 实例

直接看实例。

• 初始化卡尔曼滤波器

x = 5.3;
y = 3.6;
initialState = [x;0;y;0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);

初始化一个2D的恒速度模型，状态变量：x， vx， y， vy。即只考虑x、y方向的位移和速度，不考虑z方向以及加速度。

• 初始化真实速度、位移

vx = 0.2;
vy = 0.1;
T  = 0.5;
pos = [0:vx*T:2;5:vy*T:6]';

• 预测和更新

for k = 1:size(pos,1)
    pstates(k,:) = predict(KF,T);
    cstates(k,:) = correct(KF,pos(k,:));
end

主要用到了predict和correct两个函数实现了卡尔曼滤波中的两大主要步骤：预测和更新。

KF结果如下：

StateConvariance、ProcessNoise、MeasurementNoise分别对应KF滤波中的P、R、Q协方差矩阵。

其中，R、Q协方差矩阵为初始化后的固定值。这里都采用的是默认值。

其中R矩阵可以参考如下公式：

其中加速度方差是D(a)默认为1。（尝试过人为修改，但是无效，说明设定好模型参数后R矩阵使用的是默认参数，无法修改）；Q矩阵则可以修改；P矩阵在随着迭代改变。

控制矩阵为空，即没有输入：

• 绘制

plot(pos(:,1),pos(:,2),'k.', pstates(:,1),pstates(:,3),'+', ...
    cstates(:,1),cstates(:,3),'o')
xlabel('x [m]')
ylabel('y [m]')
grid
xt  = [x-2 pos(1,1)+0.1 pos(end,1)+0.1];
yt = [y pos(1,2) pos(end,2)];
text(xt,yt,{'First measurement','First position','Last position'})
legend('Object position', 'Predicted position', 'Corrected position')

从图中可以看出，即使刚开始初始化值较差，最终经过KF滤波后仍然会逼近真实值。

参考

1. kalman滤波理解三：协方差矩阵的计算.
2. trackingKF
3. Linear Kalman Filters