【逐行注释】MATLAB的EKF和UKF例程与详细注释|附下载链接

ops/2024/11/20 9:26:04/

在这里插入图片描述

例程简述

这是一个状态量为三维的MATLAB下的滤波程序，分成EKF（扩展卡尔曼滤波）和UKF（无迹卡尔曼滤波）两种，分别滤波后，显示滤波值的曲线、滤波误差的对比曲线、滤波误差的最大值、平均值、标准差的输出。

模型是非线性的（状态方程和观测方程都是非线性的）
我有意将模型设计得尽可能复杂一些，拿到手以后可以从难往简单的方向改，更容易上手。

部分代码与注释如下，供参考

matlab">% EKF+UKF效果对比
% date: 2023-11-07/Ver2
% 2024-6-7/Ver3：增加、优化误差输出语句
% 2024-8-25/Ver4：添加逐行注释、XYZ改为"第 维度"
clear; %清空工作区变量
clc; %清空命令行内容
close all; %关闭所有窗口（主窗口除外）
rng(0); % 设置固定的随机数种子
%% 滤波模型初始化
% 定义时间序列
t = 1:1:1000;
% 过程噪声协方差矩阵和过程噪声
Q = 1*diag([1,1,1]); %协方差矩阵
w = sqrt(Q)*randn(size(Q,1),length(t)); %过程噪声
% 观测噪声协方差矩阵和观测噪声
R = 1*diag([1,1,1]); %观测噪声协方差矩阵
v = sqrt(R)*randn(size(R,1),length(t)); %观测噪声
% 初始状态估计协方差矩阵
P0 = 1*eye(3);
% 初始化状态向量
X = zeros(3,length(t));
% 初始化扩展卡尔曼滤波状态向量
X_ekf = zeros(3,length(t));
X_ekf(1,1) = X(1,1); %给ekf滤波值赋初值
% 初始化观测值矩阵

下载链接

如需完整代码与注释，点击下面的链接下载（下载需付费，建议先看完文章，确定需要再下载）：
https://download.csdn.net/download/callmeup/89677179

代码运行情况

运行后，得到的绘图和数据输出如下：
第一幅图是三个维度的状态真实值、EKF滤波值、UKF滤波值对比：
在这里插入图片描述
第二幅图是两种滤波方法的误差曲线，对比：

第三幅图比较难理解，是误差大小的CDF（累积概率密度函数）图像：
在这里插入图片描述
CDF这个图像反映了误差的总体趋势，CDF相关的知识可见本人的另一篇文章，有详细叙述：https://blog.csdn.net/callmeup/article/details/137754111
误差的统计特性输出为误差最大值、误差平均值、误差标准差，如下：
在这里插入图片描述

各模块作用

程序分成滤波模型初始化、运动模型、EKF、UKF、绘图、输出这些部分。
滤波模型初始化，定义了PQR等滤波需要使用的参数，同时给状态量X和观测量Z这些量定义空间：
在这里插入图片描述
运动模型部分通过迭代生成真实值、未滤波的值、观测值。误差也是在这里添加的：

EKF和UKF部分为重点，不再这里展示（源码上面有相关的标注）
绘图部分，每一行也都有注释，初学者也能看懂：

输出部分使用的是fprintf函数：
在这里插入图片描述