平方根无迹卡尔曼滤波(SR-UKF)的MATLAB例程,使用三维非线性的系统

本MATLAB 代码实现了平方根无迹卡尔曼滤波(SR-UKF)算法,用于处理三维非线性状态估计问题

文章目录

运行结果

三轴状态曲线对比:

三轴误差曲线对比:

误差统计特性输出(命令行截图):

代码概述

  1. 初始化

    • 清空工作区和命令窗口,关闭所有图形窗口。
    • 定义时间序列 t 和设置过程噪声、观测噪声的协方差矩阵 (QR)。
    • 初始化状态估计协方差矩阵 P0 和状态向量 X,同时定义观测值 Z
  2. 运动模型

    • 使用循环生成真实状态 X 和未滤波状态 X_,根据给定的运动模型更新状态。
    • 生成观测值 Z,包含加入的观测噪声。
  3. 平方根UKF实现

    • 初始化滤波器的状态和协方差矩阵。
    • 在每个时间步 k 中:
      • 生成 sigma 点并计算它们的权重。
      • 对 sigma 点进行预测,计算新的状态和观测值。
      • 更新状态估计和协方差矩阵,使用卡尔曼增益结合观测更新状态。
  1. 结果绘图

    • 绘制真实值、未滤波值和滤波值的对比图。
    • 计算并绘制滤波前后的误差。
  2. 误差输出

    • 输出滤波前和 SR-UKF 后的三轴误差平均值。

代码

matlab 复制代码
% 平方根无迹卡尔曼滤波(SR-UKF),三维非线性
% 2024-12-13/Ver1

clear;clc;close all; %清空工作区、命令行,关闭小窗口
% rng(0); %固定随机种子
%% 滤波模型初始化
t = 1:1:100;% 定义时间序列
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));% 初始化状态向量
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
residue_tag = 0; %自适应标签
%% 运动模型
% 初始化未滤波的状态向量
X_ = zeros(3,length(t)); %给未滤波的值分配空间
X_(:,1) = X(:,1); %给未滤波的值赋初值
for i1 = 2:length(t)
    X(:,i1) = [X(1,i1-1) + (2.5 * X(1,i1-1) / (1 + X(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
        X(2,i1-1)+1;
        X(3,i1-1)]; %真实值
    X_(:,i1) = [X_(1,i1-1) + (2.5 * X_(1,i1-1) / (1 + X_(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
        X_(2,i1-1)+1;
        X_(3,i1-1)] + w(:,i1-1);%未滤波的值
        Z(:,i1) = [X(1,i1).^2 / 20;X(2,i1);X(3,i1)] + v(i1); %观测值
end

%% 平方根UKF
P = P0;
X_ukf=zeros(3,length(t));
X_ukf(:,1)=X(:,1);
alpha = 0.95;            % 自适应增益因子,用于更新观测噪声协方差

for k = 2 : length(t)
    Xpre = X_ukf(:,k-1);% 预测下一状态

如需帮助:

相关推荐
CV大法好11 分钟前
C#中的匿名方法、Lambda表达式、泛型委托
开发语言·c#
云空32 分钟前
《探秘 QT 5.14.1 类库的奇妙世界》
开发语言·qt
kjl53656635 分钟前
Antd react上传图片格式限制
开发语言·前端·javascript
o不ok!41 分钟前
java中带缓存的输入/输出流
java·开发语言
旺小仔.1 小时前
数据结构之二叉搜索树(Binary Search Tree)
开发语言·数据结构·c++·b树·算法·链表
夕泠爱吃糖1 小时前
如何处理对象的创建和销毁?
开发语言·c++
析木不会编程1 小时前
【C语言】特殊指针汇总
c语言·开发语言
Komorebi_99991 小时前
javaScript中slice()和splice()的用法与区别
开发语言·前端·javascript
橘颂TA1 小时前
【C++】大厂算法题(详解)
开发语言·c++·算法
严就方法1 小时前
Qt creator ,语言家功能缺失解决方法
开发语言·qt