在三维空间目标跟踪中，传感器（如相控阵雷达、光电吊舱）通常直接输出距离、方位角、俯仰角。由于球坐标系到直角坐标系的强非线性，传统的扩展卡尔曼滤波（EKF）在强非线性或大噪声下容易因雅可比矩阵线性化误差发散，而无迹卡尔曼滤波（UKF）的采样权重在维度变化时缺乏普适性。容积卡尔曼滤波（CKF）基于球面-径向容积准则，具有更高的数值稳定性与3阶多项式精确度，是解决此类问题的理想选择。配合RTS（Rauch-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);%% 参数设置dt=0.1;T=50;N=T/dt;t=(0:N-1)*dt;sensor_pos=[25;20;10];% === 3D: 传感器三维位置 ===% === 3D: 6x6 常速度(CTRV)模型 ===F=eye(6);F(1,4)=dt;F(2,5)=dt;F(3,6)=dt;q=0.5;% === 3D: 6x6 过程噪声协方差 (xyz轴独立) ===Q=q^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_r=1;sigma_az=deg2rad(1);% 方位角噪声sigma_el=deg2rad(1);% 俯仰角噪声R=diag([sigma_r^2,sigma_az^2,sigma_el^2]);% === 3D: 3x3 观测噪声 ===%% 真实轨迹与观测x_true=zeros(6,N);x_true(:,1)=[-10;0;5;1;0.5;0.2];% === 3D: 初始z=5, vz=0.2 ===z=zeros(3,N);% === 3D: 观测为[r, az, el] ===fork=2:Nx_true(:,k)=F*x_true(:,k-1)+mvnrnd(zeros(6,1),Q)';endfork=1:N dx=x_true(1,k)-sensor_pos(1);dy=x_true(2,k)-sensor_pos(2);dz=x_true(3,k)-sensor_pos(3);% === 3D: 球坐标观测模型 ===r=sqrt(dx^2+dy^2+dz^2);az=atan2(dy,dx);el=atan2(dz,sqrt(dx^2+dy^2));z(:,k)=[r;az;el]+[sigma_r*randn;sigma_az*randn;sigma_el*randn];end%% CKF 前向滤波如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
