无迹卡尔曼滤波波器程序.doc

《无迹卡尔曼滤波波器程序.doc》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。

1、clc;closeall;clearall;formatlongg;Xo=[1;0;0;0;0;0;0;0;0;0];nx=length(Xo);beta=2;elfa=1*(10^-1);lambda=(((elfa^2)*(nx))-nx);mn1=mean(Xo);%FIRSTSTEPCV1=(Xo-mn1)*(Xo-mn1)';%CV1=(Xo)*(Xo)';PO=CV1;FX=size(CV1);VTt=[1000000000]';ADP1=randn(1,200);mn2=mean(VTt);NTt=[1000000

2、000]';ADP2=randn(1,200);mn3=mean(NTt);Qo=(VTt-mn2)*(VTt-mn2)';Ro=(NTt-mn3)*(NTt-mn3)';%Qo=(VTt)*(VTt)';%Ro=(NTt)*(NTt)';FX=zeros(10,10);PAO=[POFXFX;FXQoFX;FXFXRo];Xao=[mn100]';lam=sqrt((elfa^2)*(nx));Wmo=lambda/(nx+lambda);Wmi=1/(2*(nx+lambda));Wmin=1/(2*(nx+lambda))

3、;Wco=(lambda/(nx+lambda))+(1-(elfa^2)+beta);Wci=Wmi;Wcin=Wmin;%CALCULATIONOFSIGMAPOINTS[Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33]=sigmacal(mn1,CV1,mn2,Qo,mn3,Ro,lam);%TIMEUPDATEEQUATIONS.[Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark]=TMUPDT(Wmo,Wmi

4、,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);%MEASUREMENTUPDATEEQUATIONS.[KGain,XNEW,PT1,PT2,PT3]=MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,Xo,VTt);forii=1:1:100VTt=[ADP1(ii+1)000000000]';NTt=[ADP2(ii+1)0000

5、00000]';%CV1=(XNEW)*(XNEW)';mn1=mean(XNEW);mn2=mean(VTt);mn3=mean(NTt);%Qo=(VTt)*(VTt)';%Ro=(NTt)*(NTt)';CV1=(XNEW-mn1)*(XNEW-mn1)';Qo=(VTt-mn2)*(VTt-mn2)';Ro=(NTt-mn3)*(NTt-mn3)';lam=sqrt((elfa^2)*(nx));[Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33]=sigmacal(mn1,CV1

6、,mn2,Qo,mn3,Ro,lam);[Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark]=TMUPDT(Wmo,Wmi,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);[KGain,XNEW,PT1,PT2,PT3,YNEW]=MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci

7、,Wcin,XNEW,VTt);INP1(ii)=XNEW(1,1);INP2(ii)=YNEW(1,1);endT=1:1:100;figure(1);subplot(211);plot(real(INP1));title('ORIGINALSIGNAL');subplot(212);plot(real(INP2));title('ESTIMATEDSIGNAL(UNDERNonlinearMODEL)');figure(2);plot(T,abs(INP1),T,abs(INP2));title('Combinedplot'

8、);legend('original','estimated');