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,:)));

相关推荐
DARLING Zero two♡16 分钟前
关于我、重生到500年前凭借C语言改变世界科技vlog.16——万字详解指针概念及技巧
c语言·开发语言·科技
Gu Gu Study18 分钟前
【用Java学习数据结构系列】泛型上界与通配符上界
java·开发语言
芊寻(嵌入式)40 分钟前
C转C++学习笔记--基础知识摘录总结
开发语言·c++·笔记·学习
一颗松鼠1 小时前
JavaScript 闭包是什么?简单到看完就理解!
开发语言·前端·javascript·ecmascript
有梦想的咸鱼_1 小时前
go实现并发安全hashtable 拉链法
开发语言·golang·哈希算法
海阔天空_20131 小时前
Python pyautogui库:自动化操作的强大工具
运维·开发语言·python·青少年编程·自动化
天下皆白_唯我独黑1 小时前
php 使用qrcode制作二维码图片
开发语言·php
夜雨翦春韭1 小时前
Java中的动态代理
java·开发语言·aop·动态代理
小远yyds1 小时前
前端Web用户 token 持久化
开发语言·前端·javascript·vue.js
何曾参静谧1 小时前
「C/C++」C/C++ 之 变量作用域详解
c语言·开发语言·c++