三维非线性目标跟踪仿真程序，通过扩展卡尔曼滤波（EKF）对目标状态进行在线估计，并在滤波结果的基础上利用 RTS（Rauch–Tung–Striebel）平滑算法进行后向平滑，以进一步提升轨迹估计精度。运行后输出：轨迹对比、误差曲线、误差CDF以及统计指标分析，便于直观评估不同算法的性能。
原创代码，包运行成功，禁止翻卖
文章目录
- 程序详解
- 核心亮点
- 运行结果
- MATLAB源代码
程序详解
程序结构：
核心亮点
- 三维真实场景建模
支持目标在空间中的位置与速度演化，贴近无人机、雷达跟踪等实际应用场景。 - 非线性观测建模
采用“距离 + 方位角 + 俯仰角”的观测方式，更符合实际传感器（如雷达/基站）的测量特性。 - EKF实时估计
实现标准扩展卡尔曼滤波流程，可用于在线动态跟踪。 - RTS后向平滑优化
在滤波结果基础上进一步提升轨迹精度，有效降低抖动和累积误差。 - 多维度性能评估
- 时间误差曲线
- 误差CDF分布
- 平均误差 / RMSE / 最大误差 / 标准差
运行结果
轨迹对比：
误差曲线：
误差CDF曲线：
MATLAB源代码
部分代码如下：
%% 二维非线性目标跟踪,观测为距离+角度,EKF与RTS平滑% 作者: matlabfilter% 2026-04-07/Ver1clear;clc;close all;rng(0);%% 参数设置dt=0.1;T=50;N=T/dt;t=(0:N-1)*dt;% 三维观测站sensor_pos=[20;0;5];% [x;y;z] 位置可调% 状态： [x y z vx vy vz]% 状态转移矩阵F=[100dt00;0100dt0;00100dt;000100;000010;000001];% 过程噪声q=0.5;Q=q^2*[dt^4/4*eye(3),dt^3/2*eye(3);dt^3/2*eye(3),dt^2*eye(3)];% 观测噪声sigma_r=1;sigma_az=deg2rad(1);% 方位角sigma_el=deg2rad(1);% 俯仰角R=diag([sigma_r^2,sigma_az^2,sigma_el^2]);%% 真实轨迹x_true=zeros(6,N);x_true(:,1)=[-10;0;2;1;0.5;0.2];fork=2:Nx_true(:,k)=F*x_true(:,k-1)+mvnrnd(zeros(6,1),Q)';end完整代码：
https://download.csdn.net/download/callmeup/92787543
或：如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
