扩展卡尔曼滤波matlab程序.doc

文件一：：
%THISPROGRAMISFORIMPLEMENTATIONOFDISCRETETIMEPROCESSEXTENDEDKALMANFILTER
%FORGAUSSIANANDLINEARSTOCHASTICDIFFERENCEEQUATION.
%By(R.C.R.C.R),SPLABS,MPL.
%(17JULY2005).
%HelpbyAarthiNadarajanisacknowledged.
%(drawbackofEKFiswhennonlinearityishigh,wecanextendthe
%approximationtakingadditionaltermsinTaylor'sseries).

clc;closeall;clearall;

Xint_v=[1;0;0;0;0];
wk=[10000];
vk=[10000];

forii=1:1:length(Xint_v)

Ap(ii)=Xint_v(ii)*2;
W(ii)=0;
H(ii)=-sin(Xint_v(ii));
V(ii)=0;
Wk(ii)=0;

end

Uk=randn(1,200);
Qu=cov(Uk);
Vk=randn(1,200);
Qv=cov(Vk);
C=[10000];
n=100;

[YYXX]=EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

forit=1:1:length(XX)
MSE(it)=YY(it)-XX(it);
end

tt=1:1:length(XX);

figure(1);subplot(211);plot(XX);title('ORIGINALSIGNAL');
subplot(212);plot(YY);title('ESTIMATEDSIGNAL');

figure(2);plot(tt,XX,tt,YY);title('Combinedplot');legend('original','estimated');

figure(3);plot(MSE.^2);title('Meansquareerror');

子文件：：function[YY,XX]=EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

Ap(2,:)=0;

forii=1:1:length(Ap)-1
Ap(ii+1,ii)=1;
end

inx=1;
UUk=[Uk(inx);0;0;0;0];
PPk=(Xint_v*Xint_v');
VVk=[Vk(inx);0;0;0;0];
Qv=V*V';

forii=1:1:length(Xint_v)

XKk(ii,1)=Xint_v(ii)^2;%FIRSTSTEP

end

PPk=Ap*PPk*Ap';%SECONDSTEP

Kk=PPk*C'*inv((C*PPk*C')+(V*Qv*V'));%THIRDSTEP

forii=1:1:length(Xint_v)

XUPK(ii,1)=XKk(ii)^2+UUk(ii);%UPPEREQUATIONS.

Zk(ii,1)=cos(XUPK(ii))+VVk(ii);%UPPEREQUATIONS.

end

forii=1:1:length(XKk)

XBARk(ii,1)=XKk(ii)+Kk(ii)*(Zk(ii)-(cos(XKk(ii))));%FOURTHSTEP

end

II=eye(5,5);

Pk=(II-Kk*C)*PPk;%FIFTHSTEP

%--------------------------------------------------------------------------
%--------------------------------------------------------------------------

forii=1:1:n

UUk=[Uk(ii+1);0;0;0;0];
PPk=XBARk*XBARk';
VVk=[Vk(ii+1);0;0;0;0];

XKk=exp(-XBARk);%FIRSTSTEP

PPkM=Ap*PPk*Ap';%SECONDSTEP

Kk=PPkM*C'*inv((C*PPkM*C')+(V*Qv*V'));%THIRDSTEP

fornn=1:1:length(XBARk)

XUPK(nn)=exp(-XKk(nn))+UUk(nn);%UPPEREQUATIONS.

Zk(nn)=cos(XUPK(nn))+VVk(nn);%UPPEREQU