【MATLAB例程】二维卡尔曼滤波的完整代码，仿真程序，线性系统的卡尔曼滤波（KF），带有误差统计与输出，附下载链接

本文所述代码提供了一个完整的二维卡尔曼滤波实现，涵盖了从初始化、状态更新到结果可视化的全过程。适合用于学习卡尔曼滤波的基本原理和实际应用。

代码介绍

初始化

• 清理环境：使用 clear; clc; close all 清除变量、命令行和图形窗口。
• 随机种子：设置随机数种子以保证结果可重复 (rng(0)).

滤波模型初始化

• 时间序列：设置时间从 1 到 1000。
• 噪声协方差矩阵：定义过程噪声 Q 和观测噪声 R。
• 状态和观测初始化：初始化真实状态 X、未滤波状态 X_ 和观测值 Z。

运动模型

• 状态更新：
  • 真实状态 X 按照单位矩阵更新。
  • 带误差的状态 X_ 通过加入过程噪声 w 更新。
  • 观测值 Z 通过加入观测噪声 v 更新。

卡尔曼滤波迭代

• 预测步骤：

  • 预测下一个状态 Xpre 和状态协方差 PP。
  • 计算卡尔曼增益 Kk。

• 更新步骤：

  • 根据观测值更新滤波后的状态 X_kf 和协方差 P。

可视化

• 状态量曲线绘制：

  • 绘制真实值、滤波值和未滤波值的时序图，展示滤波效果。

• 误差对比：

  • 绘制滤波前后的绝对误差曲线，比较不同状态下的误差。

• 累积分布函数 (CDF)：

  • 绘制误差的 CDF 图，展示滤波前后及观测值的误差分布。

输出结果

• 误差最大值输出：输出未滤波和滤波后的各维度误差最大值，便于分析滤波效果。

运行结果

二维状态曲线：

绝对误差的曲线：

绝对误差的CDF图像，表示误差的概率累积密度，曲线越靠近左上方表示误差整体越小：

MATLAB源代码

完整代码如下：

% KF，2维
% 2025-05-13/Ver1
clear; clc; close all;% 清除变量、命令行和图形窗口
rng(0); % 设置随机数种子

%% 滤波模型初始化
t = 1:1:1000; %设置时间序列
Q = 1 * diag(ones(1,2)); % 过程噪声协方差矩阵
w = sqrt(Q) * randn(2, length(t)); % 过程噪声
R = 10 * diag(ones(1,2)); % 观测噪声协方差矩阵
v = sqrt(R) * randn(2, length(t)); % 观测噪声
P0 = 1 * eye(2); % 初始状态协方差矩阵
X = zeros(2, length(t)); % 真实状态
X_kf = zeros(2, length(t)); % 卡尔曼滤波估计的状态
Z = zeros(2, length(t)); % 观测值形式

%% 运动模型
X_ = zeros(2, length(t)); %给带误差的（未滤波的）状态量建立空间
X_(:, 1) = X(:, 1); %给带误差的状态量赋初值
for i1 = 2:length(t)
    % 真实状态更新
    X(:, i1) = eye(2)*X(:, i1-1);
    % 未滤波状态更新
    X_(:, i1) = eye(2)*X_(:, i1-1) + w(:, i1-1);
    % 观测值更新
    Z(:, i1) = eye(2)*X(:, i1) + v(:,i1);
end
X_kf(:, 1) = X_(:, 1); %EKF估计值的初值，使用未滤波的值代替

完整代码下载链接：https://download.csdn.net/download/callmeup/90836651

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