【三维轨迹目标定位，CKF+RTS，MATLAB程序】基于CKF与RTS平滑的三维非线性目标跟踪（距离+方位角+俯仰角）

在三维空间目标跟踪中传感器如相控阵雷达、光电吊舱通常直接输出距离、方位角、俯仰角。由于球坐标系到直角坐标系的强非线性传统的扩展卡尔曼滤波EKF在强非线性或大噪声下容易因雅可比矩阵线性化误差发散而无迹卡尔曼滤波UKF的采样权重在维度变化时缺乏普适性。容积卡尔曼滤波CKF基于球面-径向容积准则具有更高的数值稳定性与3阶多项式精确度是解决此类问题的理想选择。配合RTSRauch-Tung-Striebel逆向平滑器可进一步利用未来信息显著提升轨迹精度。原创代码包运行成功文章目录概述真数据生成器CKF 前向滤波引擎RTS 逆向平滑器多维可视化与统计评估运行结果展示MATLAB源代码概述真数据生成器模拟真实物理运动规律三维常速度模型独立注入 x/y/z 三轴过程噪声。传感器侧严格遵循球坐标观测模型叠加测距与测角高斯白噪声高度还原工程实测数据特征。CKF 前向滤波引擎基于固定容积点结构预计算避免循环内重复分配内存。通过状态点传播与非线性观测映射自动完成交叉协方差计算与卡尔曼增益更新。结构通用维度自适应切换无需改核心逻辑。RTS 逆向平滑器纯线性状态转移下的最优后验估计器。利用末端滤波结果反向递推逐帧修正历史状态与协方差。无需修改滤波器内核无缝嵌入即可享受精度跃升。多维可视化与统计评估3D 空间轨迹观测站、真值、直接转换、CKF、RTS 同框对比起点终点自动标记误差时序分析实时对比滤波/平滑误差叠加理论 3σ 包络验证一致性概率分布与箱线图CDF 曲线直观展示误差收敛区间箱线图快速识别离群值终端统计面板自动打印 RMSE、最大误差、标准差及 50/68/90/95/99 分位数运行结果展示轨迹图误差曲线误差CDF曲线部分代码与运行后的命令行截图MATLAB源代码演示视频RTSCKF三维无BGM部分代码如下%% 三维非线性目标跟踪,观测为距离方位角俯仰角,CKF与RTS平滑% 作者: matlabfilter% 2026-04-05/Ver1clear;clc;close all;rng(0);%% 参数设置dt0.1;T50;NT/dt;t(0:N-1)*dt;sensor_pos[25;20;10];% 3D: 传感器三维位置 % 3D: 6x6 常速度(CTRV)模型 Feye(6);F(1,4)dt;F(2,5)dt;F(3,6)dt;q0.5;% 3D: 6x6 过程噪声协方差 (xyz轴独立) Qq^2*[dt^4/400dt^3/200;0dt^4/400dt^3/20;00dt^4/400dt^3/2;dt^3/200dt^200;0dt^3/200dt^20;00dt^3/200dt^2];sigma_r1;sigma_azdeg2rad(1);% 方位角噪声sigma_eldeg2rad(1);% 俯仰角噪声Rdiag([sigma_r^2,sigma_az^2,sigma_el^2]);% 3D: 3x3 观测噪声 %% 真实轨迹与观测x_truezeros(6,N);x_true(:,1)[-10;0;5;1;0.5;0.2];% 3D: 初始z5, vz0.2 zzeros(3,N);% 3D: 观测为[r, az, el] fork2:Nx_true(:,k)F*x_true(:,k-1)mvnrnd(zeros(6,1),Q);endfork1:N dxx_true(1,k)-sensor_pos(1);dyx_true(2,k)-sensor_pos(2);dzx_true(3,k)-sensor_pos(3);% 3D: 球坐标观测模型 rsqrt(dx^2dy^2dz^2);azatan2(dy,dx);elatan2(dz,sqrt(dx^2dy^2));z(:,k)[r;az;el][sigma_r*randn;sigma_az*randn;sigma_el*randn];end%% CKF 前向滤波如需帮助或有导航、定位滤波相关的代码定制需求请点击下方卡片联系作者