IEKF.rar_atomicdl3_iekf_卡尔曼 模型_迭代卡尔曼_非线性滤波

clear;clc;close all; % Extended Kalman Filtering d=1; %Dimension Q=0.1; %System noise variance R=1; %Measurement noise N=100; %Number of Iteration Q_cor=Q*eye(d); %Process noise covariance R_cor=R*eye(d); %Measurement noise covaraince p=eye(d); %Error covariance s=0; %Actual initial input x=s+sqrt(Q)*randn; %Initial input for EKF xv=zeros(d,N); sv=zeros(d,N); zv=zeros(d,N); xv(1)=x; for k=1:1:N y(k)=(s.^2)/20+sqrt(R)*randn; % Measurements zv(k)=x^2/20+sqrt(0)*randn; [x,p]=iter_exkalman(x,p,y(k),Q_cor,R_cor,k); %Iterative EKF Process sv(k)=x; s=s/2+(25*s)./(1+s.^2)+8*cos(1.2*(k+1))+sqrt(Q)*randn; xv(k)=s; end figure; plot(1:N,xv,'b-'); hold on; plot(1:N,sv,'r--'); legend('Noisey state','Estimated state'); xlabel('Time(s)');ylabel('Amplitude'); title(['System noise: Q=',num2str(Q)]); figure; plot(1:N,y,'b-'); hold on; plot(1:N,zv,'k--'); legend('Noisey measurement','Estimated Measurement'); xlabel('Time(s)');ylabel('Amplitude'); title(['System noise: R=',num2str(R),' Q=',num2str(Q)]); disp('Mean difference of Estimated and Actual state value:'); mean(sv-xv) disp('Standard deviation of Estimated and Actual state difference value:'); std(sv-xv)