【MATLAB例程】线性卡尔曼滤波的程序，三维状态量和观测量，较为简单，可用于理解多维KF，附代码下载链接

本文所述代码实现了一个 三维状态的扩展卡尔曼滤波 (Extended Kalman Filter, EKF) 算法。通过生成过程噪声和观测噪声，对真实状态进行滤波估计，同时对比了滤波前后状态量的误差和误差累积分布曲线。

简介

代码分为以下几个部分：

1. 初始化部分

   • 清理工作区环境，设置随机数种子，定义时间序列。
   • 定义过程噪声协方差矩阵 Q 和观测噪声协方差矩阵 R。
   • 初始化真实状态矩阵 X、观测值矩阵 Z 和滤波估计状态矩阵 X_kf。

2. 运动模型

   • 模拟真实状态、未滤波状态（带噪声）以及观测值的变化，生成数据序列。

3. 扩展卡尔曼滤波 ($EKF$)

   • 实现 EKF 的预测和更新步骤，逐步对状态进行滤波估计。
   • 计算状态协方差矩阵的更新和卡尔曼增益。

4. 绘图

   • 绘制滤波前后状态量的对比曲线。
   • 绘制滤波前后绝对误差的对比曲线。
   • 绘制误差的累积概率分布 ($CDF$) 对比图。

5. 误差输出

   • 计算并输出滤波前和滤波后各维度的最大误差值。

运行结果

• 状态量曲线：
  

• 误差曲线：
  

• 误差CDF曲线（越靠近左上表示误差整体越小）：
  

MATLAB源代码

部分代码如下：

% KF，3维
% 2025-05-12/Ver1
clear; clc; close all;% 清除变量、命令行和图形窗口
rng(0); % 设置随机数种子%% 滤波模型初始化
t = 1:1:1000; %设置时间序列
Q = 1 * diag(ones(1,3)); % 过程噪声协方差矩阵
w = sqrt(Q) * randn(3, length(t)); % 过程噪声
R = 10 * diag(ones(1,3)); % 观测噪声协方差矩阵
v = sqrt(R) * randn(3, length(t)); % 观测噪声
P0 = 1 * eye(3); % 初始状态协方差矩阵
X = zeros(3, length(t)); % 真实状态
X_kf = zeros(3, length(t)); % 扩展卡尔曼滤波估计的状态
Z = zeros(3, length(t)); % 观测值形式%% 运动模型
X_ = zeros(3, length(t)); %给带误差的（未滤波的）状态量建立空间
X_(:, 1) = X(:, 1); %给带误差的状态量赋初值

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

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