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