自适应容积卡尔曼滤波|（自适应CKF）的MATLAB源代码

news2024/11/15 12:13:12

介绍

容积卡尔曼滤波在理论上拥有比UKF更高的精度和稳定性，本自适应算法通过对观测残差的计算，在观测协方差R不准确或无法获得时，对R进行调节，以起到降低估计误差的作用。

模型

使用的是三维的非线性模型，经过适当修改，可用于组合导航等领域。
三轴滤波结果如下：
在这里插入图片描述

代码组成部分

只有一个m文件，方便运行。由以下几个部分组成：
在这里插入图片描述

运行结果

单轴的误差对比：
在这里插入图片描述
未滤波>CKF>ACKF（A是adaptive，自适应）
三轴误差对比图像：

程序部分代码

给出部分代码，以便读者获取运动模型

% CKF与ACKF效果对比，自适应调节R
% Evand©2024
% 2024-5-26/Ver1/
clear;clc;close all;
rng(0);
%% 滤波模型初始化
t = 1:1:1000;
Q0 = 1*diag([1,1,1]);w=sqrt(Q0)*randn(size(Q0,1),length(t));
R0 = 1*diag([1,1,1]);v=sqrt(R0)*randn(size(R0,1),length(t));
P0 = 1*eye(3);
X=zeros(3,length(t));
X_ekf=zeros(3,length(t));
X_ekf(:,1)=X(:,1);
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
%% 运动模型
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