% 不含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,p2T-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,:)));