卡尔曼滤波的运动目标检测算法
基于卡尔曼滤波的运动目标检测算法
卡尔曼滤波是一种高效的递归算法，特别适用于运动目标检测和跟踪。
卡尔曼滤波原理
卡尔曼滤波通过预测-校正两个阶段估计系统状态：
- 预测阶段：根据系统模型预测下一时刻状态\[\hat{x}_k^- = A\hat{x}_{k-1} + Bu_k \]\[P_k^- = AP_{k-1}A^T + Q \]
- 校正阶段：利用观测值更新预测\[K_k = P_k^-H^T(HP_k^-H^T + R)^{-1} \]\[\hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-) \]\[P_k = (I - K_kH)P_k^- \]
MATLAB实现代码
%% 基于卡尔曼滤波的运动目标检测
clc; clear; close all;
% 设置随机种子保证结果可复现
rng(42);
%% 参数设置
sim_time = 10; % 仿真时间(秒)
dt = 0.1; % 时间步长(秒)
num_steps = sim_time/dt;% 总步数
% 目标初始状态 [x, y, vx, vy]
initial_state = [0; 0; 2; 1];
% 卡尔曼滤波器参数
A = [1 0 dt 0; % 状态转移矩阵
0 1 0 dt;
0 0 1 0;
0 0 0 1];
H = [1 0 0 0; % 观测矩阵
0 1 0 0];
Q = diag([0.1, 0.1, 0.5, 0.5]); % 过程噪声协方差
R = diag([1, 1]); % 观测噪声协方差
% 创建视频对象用于记录结果
video_output = VideoWriter('kalman_tracking.avi');
video_output.FrameRate = 10;
open(video_output);
%% 初始化目标真实轨迹和观测数据
true_states = zeros(4, num_steps);
measurements = zeros(2, num_steps);
% 生成真实轨迹 (正弦运动)
for k = 1:num_steps
t = k*dt;
true_states(1,k) = 10*sin(0.5*t); % x位置
true_states(2,k) = 5*cos(0.8*t); % y位置
true_states(3,k) = 5*cos(0.5*t); % x速度
true_states(4,k) = -4*sin(0.8*t); % y速度
% 添加观测噪声
measurements(:,k) = H * true_states(:,k) + sqrt(R)*randn(2,1);
end
%% 卡尔曼滤波初始化
x_est = [measurements(1,1); measurements(2,1); 0; 0]; % 初始状态估计
P_est = diag([10, 10, 5, 5]); % 初始估计协方差
% 存储估计结果
estimated_states = zeros(4, num_steps);
estimated_states(:,1) = x_est;
%% 卡尔曼滤波主循环
for k = 2:num_steps
% --- 预测阶段 ---
x_pred = A * x_est; % 状态预测
P_pred = A * P_est * A' + Q; % 协方差预测
% --- 更新阶段 ---
K = P_pred * H' / (H * P_pred * H' + R); % 卡尔曼增益计算
x_est = x_pred + K * (measurements(:,k) - H * x_pred); % 状态更新
P_est = (eye(4) - K * H) * P_pred; % 协方差更新
% 存储估计结果
estimated_states(:,k) = x_est;
% --- 可视化 ---
figure(1); clf;
hold on; grid on; box on;
% 绘制轨迹
plot(true_states(1,1:k), true_states(2,1:k), 'b-', 'LineWidth', 1.5, 'DisplayName', '真实轨迹');
plot(measurements(1,1:k), measurements(2,1:k), 'ro', 'MarkerSize', 4, 'DisplayName', '观测点');
plot(estimated_states(1,1:k), estimated_states(2,1:k), 'g--', 'LineWidth', 1.5, 'DisplayName', '卡尔曼估计');
% 当前时刻标记
plot(true_states(1,k), true_states(2,k), 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b');
plot(measurements(1,k), measurements(2,k), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
plot(estimated_states(1,k), estimated_states(2,k), 'go', 'MarkerSize', 12, 'MarkerFaceColor', 'g');
% 添加卡尔曼增益指示器
plot([true_states(1,k), measurements(1,k)], [true_states(2,k), measurements(2,k)], 'r:', 'LineWidth', 0.5);
plot([true_states(1,k), estimated_states(1,k)], [true_states(2,k), estimated_states(2,k)], 'g-', 'LineWidth', 1.5);
% 设置图形属性
title(sprintf('卡尔曼滤波目标跟踪 (时间: %.1f秒)', k*dt));
xlabel('X位置 (m)'); ylabel('Y位置 (m)');
legend('Location', 'best');
axis equal;
axis([-15 15 -8 8]);
% 添加协方差椭圆
position_cov = P_est(1:2,1:2);
error_ellipse(position_cov, [x_est(1), x_est(2)], 'conf', 0.95, 'style', 'g-');
% 记录帧
frame = getframe(gcf);
writeVideo(video_output, frame);
pause(0.01); % 控制显示速度
end
% 关闭视频文件
close(video_output);
%% 结果分析
figure(2);
subplot(2,1,1);
hold on; grid on;
plot(dt:dt:sim_time, true_states(1,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '真实X位置');
plot(dt:dt:sim_time, measurements(1,:), 'ro', 'MarkerSize', 4, 'DisplayName', '观测X位置');
plot(dt:dt:sim_time, estimated_states(1,:), 'g--', 'LineWidth', 1.5, 'DisplayName', '估计X位置');
title('X方向位置跟踪');
xlabel('时间 (秒)'); ylabel('位置 (m)');
legend;
subplot(2,1,2);
hold on; grid on;
plot(dt:dt:sim_time, true_states(2,:), 'b-', 'LineWidth', 1.5, 'DisplayName', '真实Y位置');
plot(dt:dt:sim_time, measurements(2,:), 'ro', 'MarkerSize', 4, 'DisplayName', '观测Y位置');
plot(dt:dt:sim_time, estimated_states(2,:), 'g--', 'LineWidth', 1.5, 'DisplayName', '估计Y位置');
title('Y方向位置跟踪');
xlabel('时间 (秒)'); ylabel('位置 (m)');
legend;
%% 计算跟踪误差
position_error = sqrt((true_states(1,:) - estimated_states(1,:)).^2 + ...
(true_states(2,:) - estimated_states(2,:)).^2);
velocity_error = sqrt((true_states(3,:) - estimated_states(3,:)).^2 + ...
(true_states(4,:) - estimated_states(4,:)).^2);
figure(3);
subplot(2,1,1);
plot(dt:dt:sim_time, position_error, 'b-', 'LineWidth', 1.5);
title('位置跟踪误差');
xlabel('时间 (秒)'); ylabel('误差 (m)'); grid on;
subplot(2,1,2);
plot(dt:dt:sim_time, velocity_error, 'r-', 'LineWidth', 1.5);
title('速度跟踪误差');
xlabel('时间 (秒)'); ylabel('误差 (m/s)'); grid on;
fprintf('平均位置误差: %.4f m\n', mean(position_error));
fprintf('最大位置误差: %.4f m\n', max(position_error));
fprintf('平均速度误差: %.4f m/s\n', mean(velocity_error));
%% 辅助函数：绘制协方差椭圆
function error_ellipse(covariance, center, varargin)
% 解析可选参数
p = inputParser;
addParameter(p, 'conf', 0.95, @(x) x>0 && x<1);
addParameter(p, 'style', 'r-', @ischar);
parse(p, varargin{:});
conf = p.Results.conf;
style = p.Results.style;
% 计算椭圆参数
[V, D] = eig(covariance);
[D, order] = sort(diag(D), 'descend');
V = V(:, order);
% 计算卡方分布的临界值
chi2_val = sqrt(chi2inv(conf, 2));
% 椭圆参数
theta = linspace(0, 2*pi, 100);
a = chi2_val * sqrt(D(1));
b = chi2_val * sqrt(D(2));
% 椭圆点
ellipse = [a*cos(theta); b*sin(theta)];
ellipse = V * ellipse;
% 绘制椭圆
plot(center(1) + ellipse(1,:), center(2) + ellipse(2,:), style, 'LineWidth', 1.5);
end
代码功能详解
1. 参数设置
- 仿真时间：10秒
- 时间步长：0.1秒
- 目标初始状态：[x, y, vx, vy]
- 卡尔曼滤波器参数：
- 状态转移矩阵A
- 观测矩阵H
- 过程噪声协方差Q
- 观测噪声协方差R
2. 目标轨迹生成
- 生成正弦运动轨迹：
x = 10*sin(0.5t),y = 5*cos(0.8t) - 添加高斯观测噪声模拟实际传感器测量
3. 卡尔曼滤波实现
-
初始化：
- 初始状态估计：使用第一次观测值
- 初始协方差矩阵：设置为对角阵
-
预测阶段：
x_pred = A * x_est; P_pred = A * P_est * A' + Q; -
更新阶段：
K = P_pred * H' / (H * P_pred * H' + R); x_est = x_pred + K * (measurements(:,k) - H * x_pred); P_est = (eye(4) - K * H) * P_pred;
参考代码 基于卡尔曼滤波算法的运动目标检测 youwenfan.com/contentac/45685.html，做视频车道线检测，动态车辆识别，行人检测的朋友可以参考一下
此实现完整展示了卡尔曼滤波在运动目标检测中的应用，通过调整模型参数和噪声协方差矩阵，可以适应不同的应用场景和目标运动特性。
浙公网安备 33010602011771号