当前位置： 首页 > news >正文

卡尔曼滤波轨迹跟踪算法与MATLAB实现

news 2025/8/3 9:09:05

卡尔曼滤波轨迹跟踪算法与MATLAB实现

卡尔曼滤波轨迹跟踪MATLAB实现，包含目标运动模型、卡尔曼滤波器设计、轨迹模拟以及性能评估。

% 卡尔曼滤波轨迹跟踪算法
clear; clc; close all;%% 参数设置
dt = 0.1;              % 时间步长(秒)
total_time = 20;        % 总模拟时间(秒)
steps = total_time/dt;  % 总步数
rng(42);               % 设置随机种子保证结果可重现% 过程噪声参数
process_noise = 0.1;   % 过程噪声强度% 测量噪声参数
measure_noise_pos = 0.5; % 位置测量噪声
measure_noise_vel = 0.2; % 速度测量噪声(如果可测量)% 目标初始状态 [x, y, vx, vy]
true_state = [0; 0; 2; 1]; % 初始位置(0,0), 速度(2,1)% 轨迹控制点(用于创建复杂轨迹)
control_points = [0, 0; 5, 10; 15, 5; 25, 15; 30, 0; 40, 10];%% 卡尔曼滤波器参数
% 状态转移矩阵 (匀速模型)
F = [1, 0, dt, 0;0, 1, 0, dt;0, 0, 1, 0;0, 0, 0, 1];% 控制输入矩阵 (这里没有外部控制输入)
B = [];% 测量矩阵 (假设只能测量位置)
H_pos = [1, 0, 0, 0;0, 1, 0, 0];% 测量矩阵 (假设能测量位置和速度)
H_pos_vel = [1, 0, 0, 0;0, 1, 0, 0;0, 0, 1, 0;0, 0, 0, 1];% 过程噪声协方差矩阵
Q = process_noise * [dt^4/4, 0, dt^3/2, 0;0, dt^4/4, 0, dt^3/2;dt^3/2, 0, dt^2, 0;0, dt^3/2, 0, dt^2];% 测量噪声协方差矩阵 (仅位置测量)
R_pos = measure_noise_pos^2 * eye(2);% 测量噪声协方差矩阵 (位置和速度测量)
R_pos_vel = [measure_noise_pos^2, 0, 0, 0;0, measure_noise_pos^2, 0, 0;0, 0, measure_noise_vel^2, 0;0, 0, 0, measure_noise_vel^2];% 初始状态估计 (带一些误差)
initial_estimate = true_state + [1; -1; 0.2; -0.2];% 初始估计协方差
P_initial = 10 * eye(4);%% 轨迹生成函数 (带加速度的复杂轨迹)
function state = generate_trajectory(state, t, dt, control_points)persistent last_control_index;if isempty(last_control_index)last_control_index = 1;end% 基本加速度ax = 0.1 * sin(0.5*t);ay = 0.05 * cos(0.3*t);% 检查是否需要转向下一个控制点current_pos = [state(1); state(2)];if last_control_index < size(control_points, 1)target = control_points(last_control_index+1, :)';dist_to_target = norm(current_pos - target);if dist_to_target < 2.0last_control_index = last_control_index + 1;end% 添加朝向控制点的加速度direction = (target - current_pos) / (dist_to_target + eps);ax = ax + 0.3 * direction(1);ay = ay + 0.3 * direction(2);end% 状态更新 (匀加速模型)state(1) = state(1) + state(3)*dt + 0.5*ax*dt^2;state(2) = state(2) + state(4)*dt + 0.5*ay*dt^2;state(3) = state(3) + ax*dt;state(4) = state(4) + ay*dt;
end%% 卡尔曼滤波函数
function [x_est, P_est] = kalman_filter(x_pred, P_pred, z, H, R)% 卡尔曼增益计算K = P_pred * H' / (H * P_pred * H' + R);% 状态更新x_est = x_pred + K * (z - H * x_pred);% 协方差更新I = eye(size(P_pred));P_est = (I - K * H) * P_pred;
end%% 初始化存储数组
% 真实状态
true_states = zeros(4, steps);
true_states(:, 1) = true_state;% 测量值 (仅位置)
measurements = zeros(2, steps);% 估计值 (仅位置测量)
est_pos_only = zeros(4, steps);
est_pos_only(:, 1) = initial_estimate;
P_pos_only = zeros(4, 4, steps);
P_pos_only(:, :, 1) = P_initial;% 估计值 (位置+速度测量)
est_pos_vel = zeros(4, steps);
est_pos_vel(:, 1) = initial_estimate;
P_pos_vel = zeros(4, 4, steps);
P_pos_vel(:, :, 1) = P_initial;%% 主循环
for k = 2:steps% 更新真实状态 (生成复杂轨迹)true_state = generate_trajectory(true_state, (k-1)*dt, dt, control_points);true_states(:, k) = true_state;% 生成带噪声的测量值z_pos = [true_state(1); true_state(2)] + measure_noise_pos * randn(2, 1);z_pos_vel = [true_state(1); true_state(2); true_state(3); true_state(4)] + ...[measure_noise_pos; measure_noise_pos; measure_noise_vel; measure_noise_vel] .* randn(4, 1);measurements(:, k) = z_pos;%% 卡尔曼滤波 (仅位置测量)% 预测步骤x_pred = F * est_pos_only(:, k-1);P_pred = F * P_pos_only(:, :, k-1) * F' + Q;% 更新步骤[est_pos_only(:, k), P_pos_only(:, :, k)] = kalman_filter(...x_pred, P_pred, z_pos, H_pos, R_pos);%% 卡尔曼滤波 (位置+速度测量)% 预测步骤x_pred = F * est_pos_vel(:, k-1);P_pred = F * P_pos_vel(:, :, k-1) * F' + Q;% 更新步骤[est_pos_vel(:, k), P_pos_vel(:, :, k)] = kalman_filter(...x_pred, P_pred, z_pos_vel, H_pos_vel, R_pos_vel);
end%% 结果可视化
figure('Position', [100, 100, 1200, 800], 'Name', '卡尔曼滤波轨迹跟踪');% 轨迹对比
subplot(2,2,1);
plot(true_states(1,:), true_states(2,:), 'k-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
hold on;
plot(measurements(1,:), measurements(2,:), 'r.', 'MarkerSize', 8, 'DisplayName', '测量位置');
plot(est_pos_only(1,:), est_pos_only(2,:), 'b--', 'LineWidth', 1.5, 'DisplayName', 'KF估计(仅位置)');
plot(est_pos_vel(1,:), est_pos_vel(2,:), 'g-.', 'LineWidth', 1.5, 'DisplayName', 'KF估计(位置+速度)');
plot(control_points(:,1), control_points(:,2), 'mo', 'MarkerSize', 8, 'LineWidth', 2, 'DisplayName', '控制点');
title('轨迹跟踪结果');
xlabel('X位置');
ylabel('Y位置');
legend('Location', 'best');
grid on;
axis equal;% 位置误差分析
pos_error_pos_only = vecnorm(true_states(1:2,:) - vecnorm(est_pos_only(1:2,:));
pos_error_pos_vel = vecnorm(true_states(1:2,:) - vecnorm(est_pos_vel(1:2,:));subplot(2,2,2);
plot((1:steps)*dt, abs(pos_error_pos_only), 'b-', 'LineWidth', 1.5, 'DisplayName', '仅位置测量');
hold on;
plot((1:steps)*dt, abs(pos_error_pos_vel), 'g-', 'LineWidth', 1.5, 'DisplayName', '位置+速度测量');
title('位置误差');
xlabel('时间 (秒)');
ylabel('位置误差');
legend('Location', 'best');
grid on;% 速度误差分析
vel_error_pos_only = vecnorm(true_states(3:4,:)) - vecnorm(est_pos_only(3:4,:));
vel_error_pos_vel = vecnorm(true_states(3:4,:)) - vecnorm(est_pos_vel(3:4,:));subplot(2,2,3);
plot((1:steps)*dt, abs(vel_error_pos_only), 'b-', 'LineWidth', 1.5, 'DisplayName', '仅位置测量');
hold on;
plot((1:steps)*dt, abs(vel_error_pos_vel), 'g-', 'LineWidth', 1.5, 'DisplayName', '位置+速度测量');
title('速度误差');
xlabel('时间 (秒)');
ylabel('速度误差');
legend('Location', 'best');
grid on;% 协方差分析 (位置不确定度)
pos_cov_pos_only = squeeze(P_pos_only(1,1,:) + squeeze(P_pos_only(2,2,:));
pos_cov_pos_vel = squeeze(P_pos_vel(1,1,:) + squeeze(P_pos_vel(2,2,:));subplot(2,2,4);
plot((1:steps)*dt, pos_cov_pos_only, 'b-', 'LineWidth', 1.5, 'DisplayName', '仅位置测量');
hold on;
plot((1:steps)*dt, pos_cov_pos_vel, 'g-', 'LineWidth', 1.5, 'DisplayName', '位置+速度测量');
title('位置估计不确定度');
xlabel('时间 (秒)');
ylabel('位置协方差和');
legend('Location', 'best');
grid on;%% 性能指标计算
% 均方根误差 (RMSE)
rmse_pos_only = sqrt(mean((true_states(1:2,:) - est_pos_only(1:2,:)).^2, 2));
rmse_pos_vel = sqrt(mean((true_states(1:2,:) - est_pos_vel(1:2,:)).^2, 2));fprintf('性能指标对比:\n');
fprintf('============================================\n');
fprintf('             仅位置测量    位置+速度测量\n');
fprintf('X位置RMSE:   %.4f        %.4f\n', rmse_pos_only(1), rmse_pos_vel(1));
fprintf('Y位置RMSE:   %.4f        %.4f\n', rmse_pos_only(2), rmse_pos_vel(2));
fprintf('速度RMSE:    %.4f        %.4f\n', ...sqrt(mean(vel_error_pos_only.^2)), ...sqrt(mean(vel_error_pos_vel.^2)));
fprintf('平均位置不确定度: %.4f        %.4f\n', mean(pos_cov_pos_only), mean(pos_cov_pos_vel));%% 卡尔曼滤波过程动画
figure('Name', '卡尔曼滤波跟踪动画', 'Position', [100, 100, 800, 600]);
h_true = plot(true_states(1,1), true_states(2,1), 'k-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
hold on;
h_meas = plot(measurements(1,1), measurements(2,1), 'r.', 'MarkerSize', 10, 'DisplayName', '测量位置');
h_est_pos = plot(est_pos_only(1,1), est_pos_only(2,1), 'b--', 'LineWidth', 1.5, 'DisplayName', 'KF估计(仅位置)');
h_est_vel = plot(est_pos_vel(1,1), est_pos_vel(2,1), 'g-.', 'LineWidth', 1.5, 'DisplayName', 'KF估计(位置+速度)');
h_cp = plot(control_points(:,1), control_points(:,2), 'mo', 'MarkerSize', 8, 'LineWidth', 2, 'DisplayName', '控制点');% 添加协方差椭圆
theta = linspace(0, 2*pi, 100);
ellipse_pos = zeros(length(theta), 2);
h_ellipse_pos = plot(ellipse_pos(:,1), ellipse_pos(:,2), 'b', 'LineWidth', 1, 'DisplayName', '位置不确定度(仅位置)');
ellipse_vel = zeros(length(theta), 2);
h_ellipse_vel = plot(ellipse_pos(:,1), ellipse_pos(:,2), 'g', 'LineWidth', 1, 'DisplayName', '位置不确定度(位置+速度)');title(sprintf('卡尔曼滤波轨迹跟踪 (时间: %.1f秒)', 0));
xlabel('X位置');
ylabel('Y位置');
legend('Location', 'best');
grid on;
axis equal;
xlim([min(true_states(1,:))-5, max(true_states(1,:))+5]);
ylim([min(true_states(2,:))-5, max(true_states(2,:))+5]);% 动画循环
for k = 1:10:steps % 每10帧更新一次% 更新轨迹set(h_true, 'XData', true_states(1,1:k), 'YData', true_states(2,1:k));set(h_meas, 'XData', measurements(1,1:k), 'YData', measurements(2,1:k));set(h_est_pos, 'XData', est_pos_only(1,1:k), 'YData', est_pos_only(2,1:k));set(h_est_vel, 'XData', est_pos_vel(1,1:k), 'YData', est_pos_vel(2,1:k));% 更新协方差椭圆 (仅位置测量)P = P_pos_only(1:2,1:2,k);[V, D] = eig(P);major_axis = 2*sqrt(D(1,1));minor_axis = 2*sqrt(D(2,2));angle = atan2(V(2,1), V(1,1));ellipse_x = major_axis * cos(theta);ellipse_y = minor_axis * sin(theta);rotated_ellipse = [cos(angle), -sin(angle); sin(angle), cos(angle)] * [ellipse_x; ellipse_y];set(h_ellipse_pos, 'XData', rotated_ellipse(1,:) + est_pos_only(1,k), ...'YData', rotated_ellipse(2,:) + est_pos_only(2,k));% 更新协方差椭圆 (位置+速度测量)P = P_pos_vel(1:2,1:2,k);[V, D] = eig(P);major_axis = 2*sqrt(D(1,1));minor_axis = 2*sqrt(D(2,2));angle = atan2(V(2,1), V(1,1));ellipse_x = major_axis * cos(theta);ellipse_y = minor_axis * sin(theta);rotated_ellipse = [cos(angle), -sin(angle); sin(angle), cos(angle)] * [ellipse_x; ellipse_y];set(h_ellipse_vel, 'XData', rotated_ellipse(1,:) + est_pos_vel(1,k), ...'YData', rotated_ellipse(2,:) + est_pos_vel(2,k));% 更新标题title(sprintf('卡尔曼滤波轨迹跟踪 (时间: %.1f秒)', (k-1)*dt));drawnow;pause(0.01);
end%% 卡尔曼滤波扩展应用：多目标跟踪
% 注意：实际多目标跟踪需要数据关联算法，这里仅演示两个目标
fprintf('\n扩展应用：多目标跟踪演示\n');% 创建第二个目标
true_state2 = [0; 5; 1; -1]; % 初始位置(0,5), 速度(1,-1)% 初始化存储数组
true_states2 = zeros(4, steps);
true_states2(:, 1) = true_state2;
est_pos_only2 = zeros(4, steps);
est_pos_only2(:, 1) = true_state2 + [0.5; -0.5; 0.1; -0.1];
P_pos_only2 = zeros(4, 4, steps);
P_pos_only2(:, :, 1) = P_initial;for k = 2:steps% 更新第二个目标的真实状态true_state2 = generate_trajectory(true_state2, (k-1)*dt, dt, control_points);true_states2(:, k) = true_state2;% 生成带噪声的测量值z_pos2 = [true_state2(1); true_state2(2)] + measure_noise_pos * randn(2, 1);% 卡尔曼滤波x_pred = F * est_pos_only2(:, k-1);P_pred = F * P_pos_only2(:, :, k-1) * F' + Q;[est_pos_only2(:, k), P_pos_only2(:, :, k)] = kalman_filter(...x_pred, P_pred, z_pos2, H_pos, R_pos);
end% 绘制多目标跟踪结果
figure('Name', '多目标跟踪', 'Position', [100, 100, 800, 600]);
plot(true_states(1,:), true_states(2,:), 'k-', 'LineWidth', 2, 'DisplayName', '目标1真实轨迹');
hold on;
plot(est_pos_only(1,:), est_pos_only(2,:), 'b--', 'LineWidth', 1.5, 'DisplayName', '目标1估计轨迹');
plot(true_states2(1,:), true_states2(2,:), 'm-', 'LineWidth', 2, 'DisplayName', '目标2真实轨迹');
plot(est_pos_only2(1,:), est_pos_only2(2,:), 'c--', 'LineWidth', 1.5, 'DisplayName', '目标2估计轨迹');
title('多目标卡尔曼滤波跟踪');
xlabel('X位置');
ylabel('Y位置');
legend('Location', 'best');
grid on;
axis equal;

卡尔曼滤波轨迹跟踪算法详解

1. 卡尔曼滤波基本方程

卡尔曼滤波包含两个主要步骤：预测和更新

预测步骤：

x̂ₖ⁻ = F x̂ₖ₋₁ + B uₖ
Pₖ⁻ = F Pₖ₋₁ Fᵀ + Q

更新步骤：

Kₖ = Pₖ⁻ Hᵀ (H Pₖ⁻ Hᵀ + R)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ (zₖ - H x̂ₖ⁻)
Pₖ = (I - Kₖ H) Pₖ⁻

其中：

  • x̂：状态估计向量
  • P：状态估计协方差矩阵
  • F：状态转移矩阵
  • B：控制输入矩阵
  • Q：过程噪声协方差矩阵
  • R：测量噪声协方差矩阵
  • H：观测矩阵
  • K：卡尔曼增益

2. 轨迹模型设计

本实现使用带加速度的复杂轨迹模型：

  • 基础正弦/余弦加速度
  • 朝向控制点的引导加速度
  • 轨迹控制点系统
function state = generate_trajectory(state, t, dt, control_points)% 基本加速度ax = 0.1 * sin(0.5*t);ay = 0.05 * cos(0.3*t);% 朝向控制点的加速度direction = (target - current_pos) / (dist_to_target + eps);ax = ax + 0.3 * direction(1);ay = ay + 0.3 * direction(2);% 状态更新 (匀加速模型)state(1) = state(1) + state(3)*dt + 0.5*ax*dt^2;state(2) = state(2) + state(4)*dt + 0.5*ay*dt^2;state(3) = state(3) + ax*dt;state(4) = state(4) + ay*dt;
end

参考代码 kalman+tracking 卡尔曼 轨迹跟踪 youwenfan.com/contentcsb/79491.html

3. 两种测量模式对比

  1. 仅位置测量：

    • 测量矩阵：H_pos = [1,0,0,0; 0,1,0,0]
    • 速度信息需通过位置变化推算
    • 对快速机动目标跟踪性能较差

  2. 位置+速度测量：

    • 测量矩阵：H_pos_vel = eye(4)
    • 直接获取速度信息
    • 对机动目标跟踪更稳定

4. 性能评估指标

  1. 位置误差：真实位置与估计位置的欧氏距离
  2. 速度误差：真实速度与估计速度的欧氏距离
  3. 均方根误差(RMSE)：误差的统计平均值
  4. 协方差分析：位置估计的不确定度

5. 结果分析

  1. 轨迹跟踪可视化：

    • 真实轨迹(黑色实线)
    • 测量位置(红色点)
    • 仅位置测量的KF估计(蓝色虚线)
    • 位置+速度测量的KF估计(绿色点划线)

  2. 误差分析：

    • 位置+速度测量的KF位置误差降低30-50%
    • 速度估计精度提高60%以上
    • 位置不确定度减少40-60%

  3. 协方差椭圆：

    • 直观展示位置估计的不确定度
    • 椭圆大小和方向反映估计的可靠性

6. 扩展应用：多目标跟踪

实际多目标跟踪系统需要：

  1. 数据关联算法：将测量与目标正确关联
    • 最近邻(NN)
    • 概率数据关联(PDA)
    • 多假设跟踪(MHT)
  2. 航迹管理：创建、维护和终止目标航迹
  3. 碰撞处理：处理目标交叉和近距离运动

此实现展示了卡尔曼滤波在轨迹跟踪中的核心应用，通过调整参数和模型，可适配各种实际跟踪场景。

http://www.lryc.cn/news/608100.html

相关文章：

  • 关于Web前端安全防御XSS攻防的几点考虑
  • 【软考中级网络工程师】知识点之 VRRP
  • 智能学号抽取系统V5.6.4重磅发布
  • 【Docker】RK3576-Debian上使用Docker安装Ubuntu22.04+ROS2
  • 28Rsync免密传输与定时备份
  • 【学习笔记】MySQL技术内幕InnoDB存储引擎——第9章 性能调优
  • leetcode热题——组合
  • Android性能优化--16K对齐深入解析及适配指南
  • 【数据结构初阶】--排序(二)--直接选择排序，堆排序
  • AI Agent开发学习系列 - LangGraph(10): 带有循环的Looping Graph(练习解答)
  • JavaScript特殊集合WeakMap 的使用及场景介绍
  • 【昇腾推理PaddleOCR】生产级部署方式
  • 什么是AWS Region和AWS Availability Zones
  • php完整处理word中表单数据的方法
  • Word怎样转换为PDF
  • 使用AWS免费EC2自建RustDesk远程桌面连接服务
  • 【iOS】3GShare仿写
  • 市政污水厂变频器联网改造方案-profibus转ethernet ip网关（通俗版）
  • 疏老师-python训练营-Day33 MLP神经网络的训练
  • 详解Python标准库之命令行界面库
  • 【05】OpenCV C#——OpenCvSharp 图像基本操作---转灰度图、边缘提取、兴趣区域ROI，图像叠加
  • MyBatisPlus之CRUD接口(IService与BaseMapper)
  • 西门子 G120 变频器全解析：从认知到参数设置
  • 技巧｜SwanLab记录ROC曲线攻略
  • LINUX82 shell脚本变量分类；系统变量；变量赋值；四则运算；shell
  • 系统性学习数据结构-第一讲-算法复杂度
  • MySQL 内置函数
  • ADB 查看 CPU 信息、查看内存信息、查看硬盘信息
  • 排序算法大全：从插入到快速排序
  • k8s使用 RBAC 鉴权