EKF例程 matlab

% 不含IMU误差方程的EKF滤波典型程序,适用于多次滤波的第二级

% author:Evand

% date: 2023-09-20

% Ver1

clear;clc;close all;

global T

%% initial

T = 0.1; %采样率

t = [T:T:100];

Q = 0.1diag([1,1,1]);w=sqrt(Q)randn(size(Q,1),length(t));
R = 1
diag([1,1,1]);v=sqrt®randn(size(R,1),length(t));
P = 1
eye(3);
P_num = zeros(length(t),size(P,1),size(P,2)); %存放每次迭代的P
X=zeros(3,length(t));
X_ekf=zeros(3,length(t));
X_ekf(1,1)=X(1,1);
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
%% model
X_=zeros(3,length(t));
X_(:,1)=X(:,1);
for i1 = 2:length(t)
X(:,i1) = [1 * 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;

(2X(3,i1-1)+1)^0.5]; %真实值
X_(:,i1) = [1 * 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

%% EKF

for k = 2 : length(t)

Xpre = [1X_ekf(1,k-1)+ 2.5 X_ekf(1,k-1)./(1+X_ekf(1,k-1).^2) + 8 * cos(1.2*(k-1));

X_ekf(2,k-1)+1;

X_ekf(3,k-1)]+w(:,k-1);

Z_hat =[Xpre(1).^2/20;Xpre(2);Xpre(3)];

F = [1 + 2.5 * (1-X_ekf(1,k-1).^2)/((1+X_ekf(1,k-1).^2).^2),0,0;

0,1,0;

0,0,(2X_ekf(3,k-1)+1)^(-0.5)];
H = [Xpre(1)/10,0,0;
0,1,0;
0,0,1];
PP=F
PF'+Q;
Kk=PP
H'/(HPP H'+R);

X_ekf(:,k)=Xpre+Kk*(Z(:,k)-Z_hat);

P=PP-KkH PP;

end

%% display

figure;

subplot(3,1,1);

plot(t,X(1,:),t,X_ekf(1,:));

title('EKF');

legend('real','EKF');

subplot(3,1,2);

plot(t,X(2,:),t,X_ekf(2,:));

subplot(3,1,3);

plot(t,X(3,:),t,X_ekf(3,:));

figure;

subplot(3,1,1); %1st

plot(t,X_ekf(1,:)-X(1,:),t,X_(1,:)-X(1,:));

title('Err');

legend('EKF','without KF');

[~,p1] = max(abs(X_ekf(1,:)-X(1,:)));

h = gca().Children(1);

datatip(h,p1T-1,X_ekf(1,p1)-X(1,p1));
[~,p2] = max(abs(X_(1,:)-X(1,:)));
datatip(h,p2
T-1,X_(1,p2)-X(1,p2));

subplot(3,1,2); %2nd

plot(t,X_ekf(2,:)-X(2,:),t,X_(2,:)-X(2,:));

subplot(3,1,3); %3rd

plot(t,X_ekf(3,:)-X(3,:),t,X_(3,:)-X(3,:));

[~,p1] = max(abs(X_ekf(3,:)-X(3,:)));

h = gca().Children(1);

datatip(h,p1T-1,X_ekf(3,p1)-X(3,p1));
[~,p2] = max(abs(X_(3,:)-X(3,:)));
datatip(h,p2
T-1,X_(3,p2)-X(3,p2));

figure;

subplot(3,1,1); %1st

cdfplot(abs(X_ekf(1,:)-X(1,:)));

hold on

cdfplot(abs(X_(1,:)-X(1,:)));

legend('EKF','without KF');

subplot(3,1,2); %2nd

cdfplot(abs(X_ekf(2,:)-X(2,:)));

hold on

cdfplot(abs(X_(2,:)-X(2,:)));

subplot(3,1,3); %3rd

cdfplot(abs(X_ekf(3,:)-X(3,:)));

hold on

cdfplot(abs(X_(3,:)-X(3,:)));

相关推荐
Swift社区41 分钟前
在 Swift 中实现字符串分割问题:以字典中的单词构造句子
开发语言·ios·swift
没头脑的ht43 分钟前
Swift内存访问冲突
开发语言·ios·swift
没头脑的ht1 小时前
Swift闭包的本质
开发语言·ios·swift
wjs20241 小时前
Swift 数组
开发语言
stm 学习ing2 小时前
FPGA 第十讲 避免latch的产生
c语言·开发语言·单片机·嵌入式硬件·fpga开发·fpga
湫ccc3 小时前
《Python基础》之字符串格式化输出
开发语言·python
mqiqe3 小时前
Python MySQL通过Binlog 获取变更记录 恢复数据
开发语言·python·mysql
AttackingLin3 小时前
2024强网杯--babyheap house of apple2解法
linux·开发语言·python