程序讲解

状态方程、观测方程以及对应的解析雅可比矩阵均使用匿名函数定义，只需替换 f、h、F_fun、H_fun 四个句柄，即可快速测试自己的模型，无需重构整个滤波架构。

代码同时实现前向EKF实时估计与后向RTS固定区间平滑，并自动对齐数据、计算增益、修正协方差。可直接对比同一系统下的在线估计与离线平滑精度，深入理解平滑算法如何利用全部观测“修正历史”。

所有对比图形均自动生成并排版，包含置信区间填充、动态刷新、图例标注、柱顶数值标注、旋转轴标签等细节。

通过 rng(0) 固定随机数种子，保证每次运行得到的估计曲线与误差统计完全相同，便于教学演示或与他人交流对比。

运行结果

实时滤波动画，第一视角感受EKF跟踪过程
程序运行后会自动弹出一个动态窗口，真实状态（绿色曲线）、带噪声的观测（红色散点）与EKF实时估计（蓝色曲线）同步推进，可直观理解非线性滤波的预测—更新循环。

• 轨迹估计：同时展示真实值、EKF结果、RTS平滑结果，并用半透明色带绘制±1σ置信区间，不确定性大小一目了然。

• 误差曲线：EKF与RTS的估计误差同框比较，图例自动标注RMSE数值，平滑优势跃然纸上。

• RMSE柱状图和MAE柱状图、命令行窗口
  

代码

部分代码：

% 卡尔曼滤波与反向滤波示例,卡尔曼滤波与RTS平滑示例（含误差对比分析）
% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-06-02/Ver1

%% 初始化
clear; clc; close all;
rng(0); % 初始化环境
N = 100; Q = 0.01; R = 1; % 参数：步数, 过程噪声方差, 测量噪声方差
% 非线性模型定义
f = @(x) x + 0.15*sin(x); % 状态方程
h = @(x) x.^2/20; % 测量方程
F_fun = @(x) 1 + 0.15*cos(x); % 状态转移雅可比
H_fun = @(x) x/10; % 测量雅可比

x_true = zeros(1,N); x_true(1) = 2; % 生成真实轨迹初值
for k = 2:N
    x_true(k) = f(x_true(k-1)) + sqrt(Q)*randn;
end
z = h(x_true) + sqrt(R)*randn(1,N); % 生成观测

x_hat = zeros(1,N); P_post = zeros(1,N); % EKF初始化
x_pred_all = zeros(1,N); P_pred_all = zeros(1,N);
x_hat(1) = sqrt(abs(z(1)*20)); P_post(1) = 1;

%% 滤波与实时滤波动画

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

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