原创代码，拒绝AI生成。未经允许，禁止翻卖
文章目录
程序详解
简介 ：
程序模拟了二维场景下单部测角测距雷达对运动目标的实时跟踪，并基于卡尔曼滤波（KF）实现对目标轨迹的融合估计。
目标按"带加速度"的非匀速模型运动，而滤波器采用匀速运动模型，从而体现模型失配与滤波器补偿能力。雷达位于平面固定坐标处，每一帧输出带噪声的距离与方位角测量，程序将其转化为直角坐标作为观测输入。
对比 ：
除了雷达观测和KF融合结果外，代码还加入了"纯惯导（基于匀速假设的推算）"作为对比，用于展示不同方法在长时间跟踪中的误差发散与抑制能力。
结果输出 ：
输出真实轨迹、雷达量测、纯惯导估计及KF结果，并统计平均误差、RMSE以及滤波相对提升幅度。最后给出多种图形，包括轨迹对比、误差曲线及柱状统计图，便于全面评估单雷达定位方案与卡尔曼滤波在噪声环境中的性能差异。
运行结果
轨迹对比：
误差对比曲线：
结果输出：
MATLAB源代码
部分代码如下：
matlab
% 雷达测角测距定位-二维-单一雷达，轨迹用KF
% 作者：matlabfilter
% 2025-11-25/Ver1 - 添加雷达观测和纯惯导对比
clear; close all; clc;
rng(0);
%% 系统参数设置
dt = 0.01; % 时间步长 (秒)
N = 400; % 仿真步数
%% 目标初始状态
target_position = [100; 200]; % 初始位置 [x; y] (米)
target_velocity = [1; 5]; % 初始速度 [vx; vy] (米/秒)
target_acceleration = [0.5; -0.1]; % 加速度来破坏匀速模型 [ax; ay] (米/秒²)
radar_position = [100; 210]; %雷达坐标 m，可调
%% 测量噪声参数
range_noise_std = 1; % 距离测量噪声标准差 (m)
angle_noise_std = 0.1; % 角度测量噪声标准差 (rad)
%% 卡尔曼滤波器
% 状态向量: [x, y, vx, vy]'
% 状态转移矩阵 (匀速模型)
F = [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 = 0.1; % 过程噪声强度
Q = q * [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 = diag([1,1].^2);
% 初始状态估计
x_est = [target_position; target_velocity];
% 初始协方差矩阵
P = diag([1, 1, 0.5, 0.5]);
%% 纯惯导解算初始化
% 假设初始状态已知（实际中可能来自首次观测）
inertial_position = target_position;
inertial_velocity = target_velocity;
% 平均位置误差柱状图
figure;
bar_data = [mean_error_radar, mean_error_inertial, mean_error_kf];
b = bar(bar_data);
b.FaceColor = 'flat';
b.CData(1,:) = [0 0 0]; % 黑色
b.CData(2,:) = [1 0 1]; % 洋红色
b.CData(3,:) = [0 0 1]; % 蓝色
set(gca, 'XTickLabel', {'雷达观测', '纯惯导', 'KF估计'});
ylabel('平均位置误差 (米)', 'FontSize', 12);
title('平均位置误差对比', 'FontSize', 14, 'FontWeight', 'bold');
grid on;
for i = 1:length(bar_data)
text(i, bar_data(i)+max(bar_data)*0.05, sprintf('%.2f m', bar_data(i)), ...
'HorizontalAlignment', 'center', 'FontSize', 11, 'FontWeight', 'bold');
end
% 位置RMSE柱状图
figure;
rmse_data = [rmse_radar, rmse_inertial, rmse_kf];
b = bar(rmse_data);
b.FaceColor = 'flat';
b.CData(1,:) = [0 0 0]; % 黑色
b.CData(2,:) = [1 0 1]; % 洋红色
b.CData(3,:) = [0 0 1]; % 蓝色
set(gca, 'XTickLabel', {'雷达观测', '纯惯导', 'KF估计'});
ylabel('位置RMSE (米)', 'FontSize', 12);
title('位置RMSE对比', 'FontSize', 14, 'FontWeight', 'bold');
grid on;
for i = 1:length(rmse_data)
text(i, rmse_data(i)+max(rmse_data)*0.05, sprintf('%.2f m', rmse_data(i)), ...
'HorizontalAlignment', 'center', 'FontSize', 11, 'FontWeight', 'bold');
end完整代码：
https://download.csdn.net/download/callmeup/92408190
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者