Original code, no AI generation. Resale without permission is prohibited.

Program Explanation

Introduction: The program simulates real-time tracking of a moving target using a single angle and distance measuring radar in a two-dimensional scene, and implements fusion estimation of the target trajectory based on Kalman Filtering (KF). The target moves according to a “non-uniform model with acceleration”, while the filter uses a uniform motion model, thus reflecting the model mismatch and the filter’s compensation capability. The radar is located at a fixed coordinate in the plane, and each frame outputs noisy distance and azimuth angle measurements, which are converted into Cartesian coordinates as observation inputs.

Comparison: In addition to the radar observations and KF fusion results, the code also includes “pure inertial navigation (based on uniform motion assumption)” for comparison, to demonstrate the error divergence and suppression capabilities of different methods during long-term tracking.

Results Output: Outputs the true trajectory, radar measurements, pure inertial estimates, and KF results, and calculates the average error, RMSE, and the relative improvement of filtering. Finally, various graphs are provided, including trajectory comparison, error curves, and bar charts for comprehensive evaluation of the single radar localization scheme and the performance differences of Kalman filtering in noisy environments.

Running Results

Trajectory Comparison:Error Comparison Curve:Results Output:

MATLAB Source Code

Part of the code is as follows:

% Radar angle and distance measurement localization - 2D - single radar, trajectory using KF
% Author: matlabfilter (same ID), custom MATLAB code related to positioning and navigation, filtering
% 2025-11-25/Ver1 - Added radar observation and pure inertial comparison

clear; close all; clc;
rng(0);
%%  System parameter settings
dt =0.01;% Time step (seconds)
N =400;% Simulation steps

%%  Target initial state
target_position =[100;200];% Initial position [x; y] (meters)
target_velocity =[1;5];% Initial velocity [vx; vy] (meters/second)
target_acceleration =[0.5;-0.1];% Acceleration to disrupt uniform model [ax; ay] (meters/second²)
radar_position =[100;210];% Radar coordinates (m), adjustable

%% Measurement noise parameters
range_noise_std =1;% Distance measurement noise standard deviation (m)
angle_noise_std =0.1;% Angle measurement noise standard deviation (rad)

%% Kalman Filter
% State vector: [x, y, vx, vy]'
% State transition matrix (uniform motion model)
F =[1,0, dt,0;
0,1,0, dt;
0,0,1,0;
0,0,0,1];

% Observation matrix (only observing position)
H =[1,0,0,0;
0,1,0,0];

% Process noise covariance
q =0.1;% Process noise intensity
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];

% Measurement noise covariance
R =diag([1,1].^2);

% Initial state estimate
x_est =[target_position; target_velocity];

% Initial covariance matrix
P =diag([1,1,0.5,0.5]);

%% Pure inertial navigation initialization
% Assume initial state is known (may come from first observation in practice)
inertial_position = target_position;
inertial_velocity = target_velocity;


% Average position error bar chart
figure;
bar_data =[mean_error_radar, mean_error_inertial, mean_error_kf];
b =bar(bar_data);
b.FaceColor ='flat';
b.CData(1,:)=[000];% Black
b.CData(2,:)=[101];% Magenta
b.CData(3,:)=[001];% Blue
set(gca,'XTickLabel',{'Radar Observation','Pure Inertial','KF Estimate'});
ylabel('Average Position Error (meters)','FontSize',12);
title('Average Position Error Comparison','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

% Position RMSE bar chart
figure;
rmse_data =[rmse_radar, rmse_inertial, rmse_kf];
b =bar(rmse_data);
b.FaceColor ='flat';
b.CData(1,:)=[000];% Black
b.CData(2,:)=[101];% Magenta
b.CData(3,:)=[001];% Blue
set(gca,'XTickLabel',{'Radar Observation','Pure Inertial','KF Estimate'});
ylabel('Position RMSE (meters)','FontSize',12);
title('Position RMSE Comparison','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

Complete code:

https://mall.bilibili.com/neul-next/detailuniversal/detail.html?isMerchant=1&page=detailuniversal_detail&saleType=10&itemsId=13576775&loadingShow=1&noTitleBar=1&msource=merchant_share

Or scan the code:

<span>If you need help or have custom code requirements related to navigation and positioning filtering, please contact the author:</span>