轨迹跟踪代码matlab

以下是一个简单的基于Kalman滤波的目标跟踪代码示例，使用Matlab实现。该代码可以跟踪固定位置的目标，如车辆或行人的位置。请注意，这只是一个简单的示例，实际应用中可能需要根据具体场景进行修改和优化。

% 初始化Kalman滤波器
dt = 1; % 时间步长
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵
C = [1 0 0 0; 0 1 0 0]; % 观测矩阵
Q = eye(4); % 状态噪声协方差
R = eye(2); % 观测噪声协方差
P = eye(4); % 初始估计误差协方差
x = [0; 0; 0; 0]; % 初始状态
kalman = struct('A', A, 'C', C, 'Q', Q, 'R', R, 'P', P, 'x', x);

% 模拟目标移动
num_steps = 100; % 时间步数
target_pos = [50; 50]; % 目标初始位置
target_vel = [10; 5]; % 目标初始速度
target_acc = [0; 0]; % 目标加速度
target_traj = zeros(2, num_steps); % 目标轨迹
for i = 1:num_steps
    target_pos = target_pos + target_vel * dt + 0.5 * target_acc * dt^2;
    target_vel = target_vel + target_acc * dt;
    target_traj(:, i) = target_pos; 
end

% 观测目标位置
observed_traj = zeros(2, num_steps); % 观测到的目标轨迹
for i = 1:num_steps
    observed_pos = target_traj(:, i) + randn(2, 1) * sqrt(R); % 加入高斯噪声
    observed_traj(:, i) = observed_pos;
end

% 跟踪目标
tracked_traj = zeros(2, num_steps); % 跟踪到的目标轨迹
for i = 1:num_steps
    % 预测下一时刻目标状态
    x_pred = kalman.A * kalman.x;
    P_pred = kalman.A * kalman.P * kalman.A' + kalman.Q;
    
    % 更新状态估计
    y = observed_traj(:, i) - kalman.C * x_pred;
    K = P_pred * kalman.C' / (kalman.C * P_pred * kalman.C' + kalman.R);
    kalman.x = x_pred + K * y;
    kalman.P = (eye(4) - K * kalman.C) * P_pred;
    
    % 记录跟踪到的目标位置
    tracked_traj(:, i) = kalman.C * kalman.x;
end

% 可视化结果
figure;
plot(target_traj(1, :), target_traj(2, :), 'k-', 'LineWidth', 2); % 真实轨迹
hold on;
plot(observed_traj(1, :), observed_traj(2, :), 'rx'); % 观测到的轨迹
plot(tracked_traj(1, :), tracked_traj(2, :), 'bo-'); % 跟踪到的轨迹
legend('True trajectory', 'Observed trajectory', 'Tracked trajectory');
xlabel('X position');
ylabel('Y position');

这个代码基于Kalman滤波器实现，用于跟踪目标的位置，其中使用了高斯噪声来模拟观测误差。该代码的输出是跟踪到的目标位置轨迹，您可以根据实际需要进一步扩展和修改代码。