线性卡尔曼滤波（
K
a
l
m
a
n
F
i
l
t
e
r
,
K
F
Kalman Filter, KF
KalmanFilter,KF）与
R
a
u
c
h
–
T
u
n
g
–
S
t
r
i
e
b
e
l
Rauch–Tung–Striebel
Rauch–Tung–Striebel（RTS）平滑算法的完整对比仿真，以一维匀速运动模型为背景，对目标的位置和速度进行联合估计。通过构建前向滤波与后向平滑两级处理流程，系统性展示了 RTS 平滑相较于标准卡尔曼滤波在估计精度和误差稳定性方面的优势。 原创代码，非AI生成，请勿翻卖
文章目录
- 程序介绍
-
- 代码功能简介
- 前向滤波：线性卡尔曼滤波
- 后向平滑：RTS 平滑算法
- 运行结果
- MATLAB源代码
程序介绍
代码功能简介
代码实现了线性卡尔曼滤波（
K
F
KF
KF）与 Rauch–Tung–Striebel（
R
T
S
RTS
RTS）平滑算法的联合仿真与性能对比，以一维匀速运动模型为背景，对目标的位置与速度进行联合状态估计。通过“前向滤波 + 后向平滑”的两阶段处理流程，系统性展示了
R
T
S
RTS
RTS平滑在利用全时域信息、提升估计精度方面的优势。
前向滤波：线性卡尔曼滤波
代码的第一阶段采用标准线性卡尔曼滤波进行状态估计，包括预测与更新两个基本步骤。滤波器在每个时刻仅利用当前及历史观测信息对状态进行递推估计，因此具有实时性强、计算量低的特点，但在有限观测条件下，估计精度仍受噪声影响较为明显。
滤波过程中，代码同时保存预测结果与协方差信息，为后续
R
T
S
RTS
RTS平滑提供必要的中间量。
后向平滑：RTS 平滑算法
在完成前向卡尔曼滤波后，代码进一步引入
R
T
S
RTS
RTS平滑算法，对状态序列进行后向递推。RTS 平滑在每个时刻综合利用过去、当前以及未来的观测信息，对卡尔曼滤波结果进行全局修正，从而显著降低估计误差。
该过程通过前向预测协方差与滤波协方差构造平滑增益，实现对位置与速度估计的统一平滑处理。
运行结果
估计的位置和速度变化曲线，包含与真值的对比： 
误差对比曲线： 
命令行输出的RMSE对比： 
MATLAB源代码
部分代码如下：
% Rauch-Tung-Striebel (RTS)平滑与线性的卡尔曼滤波
% 背景为估计位置和速度，可修改为其他应用环境
% 作者:matlabfilter
% 2026-01-15/Ver1
clear; clc; close all;
rng(0);
%% 系统参数设置
dt = 0.1; % 采样时间间隔
T = 50; % 总时间
N = T/dt; % 总步数
t = (0:N–1)*dt; % 时间向量
% 状态向量 x = [位置; 速度]
% 状态转移矩阵 F (匀速模型)
F = [1, dt;
0, 1];
% 观测矩阵 H (只观测位置)
H = [1, 0];
% 过程噪声协方差 Q
q = 0.1;
Q = [q^3/3, q^2/2;
q^2/2, q];
% 观测噪声协方差 R
R = 1.0; % 观测噪声
%% 模拟真实数据和观测数据
x_true = zeros(2, N);
x_true(:,1) = [0; 2]; % 初始状态: 位置0, 速度2
z = zeros(1, N); % 观测值
rng(100); % 固定随机种子
for k = 2:N
x_true(:,k) = F * x_true(:,k–1) + mvnrnd([0;0], Q)';
end
for k = 1:N
z(k) = H * x_true(:,k) + sqrt(R) * randn;
end
完整代码： 如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方按钮联系作者
