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

关注

原创最新推荐文章于 2026-11-07 18:14:40 发布 394 阅读 AI 写同款· GEO 优化 ›

【MATLAB代码，多无人机路径规划6】基于RRT与动态窗口法（RRT+DWA）的多无人机二维动态路径规划与避障。订阅专栏后可查看完整代码本文基于MATLAB实现四架无人机在二维动态障碍环境中的协同路径规划，采用RRT+DWA算法，支持参数自定义。通过对比RRT+APF与RRT+DWA的避障效果，可视化轨迹、净间隙、速度加速度及航程等，输出任务完成率与碰撞统计。代码可直接运行，订阅专栏后获取完整源码，支持定制与讲解。阅读详情

在这里插入图片描述

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

程序简介

代码结构

模块	关键变量	作用
参数设置	`F, Q, R`	运动/噪声模型
轨迹生成	`x_true, z`	蒙特卡洛仿真
CKF 前向	`x_filt, P_filt, x_pred, P_pred`	实时估计
RTS 后向	`x_smooth, P_smooth`	离线平滑
可视化	轨迹图、误差时序、CDF、箱线图	综合性能分析

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

运行结果

运动轨迹：
在这里插入图片描述

误差图像：
在这里插入图片描述

命令行输出的结果：

在这里插入图片描述

程序结构：
在这里插入图片描述

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; 0];

F = [1 0 dt 0;
     0 1 0  dt;
     0 0 1  0;
     0 0 0  1];

q = 0.5;
Q = q^2 * [dt^4/4 0       dt^3/2 0;
           0       dt^4/4 0       dt^3/2;
           dt^3/2 0       dt^2   0;
           0       dt^3/2 0       dt^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);

for k = 2:N
    x_true(:,k) = F*x_true(:,k-1) + mvnrnd(zeros(4,1),Q)';
end

for k = 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);