
行人跟踪这事儿听起来简单,实际处理起来传感器噪声能把人逼疯。去年实验室用传统光
流法翻车之后,我们开始折腾卡尔曼滤波。直接上代码,先看核心部分的状态预测:
```matlab
function [x_est, P_est] = kalman_predict(x_prev, P_prev, F, Q)
x_pred = F * x_prev; % 状态预测
P_pred = F * P_prev * F' + Q; % 协方差更新
% 这里藏着个矩阵运算的坑,后面细说
x_est = x_pred;
P_est = P_pred;
end
```
状态向量 x_prev 我们设的是[x, y, vx, vy],把位置和速度都包进去。F 矩阵设计最考验经验
,刚开始用匀速模型翻车了——行人突然转向时预测直接飘了。后来改成:
```matlab
F = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1]; % 状态转移矩阵
```
这个 dt 参数调了整整两天,实测发现当摄像头帧率 30fps 时,dt=0.033 秒效果最佳。过
程噪声 Q 矩阵更玄学,试了十几种组合后发现对角矩阵[0.1, 0.1, 0.5, 0.5]效果最好。
观测更新部分有个大坑:H 矩阵维度不对齐会导致程序崩溃。正确的姿势是:
```matlab
H = [1 0 0 0;
0 1 0 0]; % 观测矩阵
function [x_corr, P_corr] = kalman_update(x_pred, P_pred, z, H, R)
y = z - H * x_pred;
S = H * P_pred * H' + R;
K = P_pred * H' / S; % 卡尔曼增益计算
x_corr = x_pred + K * y;
P_corr = (eye(4) - K * H) * P_pred;
end
```