【CKF与RTS，MATLAB例程】二维非线性目标跟踪，观测为距离+角度，滤波使用容积卡尔曼滤波，附加RTS平滑，获得高精度定位。附代码下载链接

通过模拟二维平面下目标的运动模型与传感器的距离/方位/俯仰观测，利用容积卡尔曼滤波（CKF）进行前向状态估计，并结合RTS算法进行后向平滑优化，最终对比可视化三者的轨迹与定位精度
原创代码，包运行成功。请勿翻卖

程序简介

代码结构

误差评估同时给出了 RMSE、最大误差、标准差和多个百分位点，以及3 σ 3\sigma3σ包络线，具体的运行结果如下。

运行结果

运动轨迹：

误差图像：

命令行输出的结果：

程序结构：

MATLAB源代码

部分代码如下：

%% 二维非线性目标跟踪,观测为距离+角度,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;0];F=[10dt0;010dt;0010;0001];q=0.5;Q=q^2*[dt^4/40dt^3/20;0dt^4/40dt^3/2;dt^3/20dt^20;0dt^3/20dt^2];sigma_r=1;sigma_b=deg2rad(1);R=diag([sigma_r^2,sigma_b^2]);%% 真实轨迹与观测x_true=zeros(4,N);x_true(:,1)=[-10;0;1;0.5];z=zeros(2,N);fork=2:Nx_true(:,k)=F*x_true(:,k-1)+mvnrnd(zeros(4,1),Q)';endfork=1:N dx=x_true(1,k)-sensor_pos(1);dy=x_true(2,k)-sensor_pos(2);z(:,k)=[sqrt(dx^2+dy^2);atan2(dy,dx)]+...[sigma_r*randn;sigma_b*randn];end%% CKF 前向滤波n=4;x_filt=zeros(4,N);P_filt=zeros(4,4,N);x_pred=zeros(4,N);P_pred=zeros(4,4,N);

完整代码：
https://download.csdn.net/download/callmeup/92787557

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者