扩展卡尔曼滤波的S函数实现

扩展卡尔曼滤波器的s函数编写function[sys,x0,str,ts]=sekfs(t,x,u,flag)switchflag,case0,[sys,x0,str,ts]=mdlInitializeSizes;case2,sys=mdlUpdate(t,x,u);case3,sys=mdlOutputs(t,x,u);case{1,4,9},sys=[];otherwiseerror(['Unhandledflag=',num2str(flag)]);endfunction[sys,x0,str,ts]=mdlInitializeSizessizes=simsizes;sizes.NumContStates=0;sizes.NumDiscStates=3;sizes.NumOutputs=1;sizes.NumInputs=1;sizes.DirFeedthrough=0;sizes.NumSampleTimes=1;sys=simsizes(sizes);x0=[4.500015.000015.0000];str=[];ts=1e-6;functionsys=mdlUpdate(t,x,u)globalQon;Qon=1;R=1e-2;Q=Qon*diag([1e-21e-21e-2]);G_basal=4.5;%mmol/LX_basal=15;%mU/LI_basal=15;%mU/LP1=0.028735;%min-1P2=0.028344;%min-1P3=5.035e-5;%mU/LV1=12;%Ln=5/54;%minD=5;To=1e-6;%fori=1:3%forj=1:3%P(i,j)=x(3*i+j);%end%endP=[0.0100;00.010;000.01];xp=zeros(3,size(x,2));xp(1,1)=x(1)+(-P1*(x(1)-G_basal)-(x(2)-X_basal)*x(1)+D)*To;xp(2,1)=x(2)+(-P2*(x(2)-X_basal)+P3*(x(3)-I_basal))*To;xp(3,1)=x(3)+(-n*x(3)+u(1)/V1)*To;F=[-P1-x(2),-x(1),0;0,-P2,P3;0,0,-n];P=F*P*F'+Q;H=[1,0,0];K=P*H'*inv(H*P*H'+R);D=u(1);H=xp(1,1);xp=xp+K*(D-H);P=P-K*H*P;%sys=[xp(1,1);xp(2,1);xp(3,1)];fori=1:3sys(i)=xp(i,1);end%fori=1:3%forj=1:3%sys(i,j)=P(3*i+j);%end%endfunctionsys=mdlOutputs(t,x,u)sys=x(1);Errorin'untitled/S-Function'whileexecutingM-FileS-function'sekfs',flag=2(update),attime0.MATLABerrormessage:Innermatrixdimensionsmustagree.哪里错了？最佳答案H=[1,0,0];K=P*H'*inv(H*P*H'+R);D=u(1);H=xp(1,1);xp=xp+K*(D-H);P=P-K*H*P;H是一个3*1的矩阵，但在下面H=xp(1,1)，这时又变成一个数了，后面xp=xp+K*(D-H);没有问题。但是P=P-K*H*P;这里就出现了矩阵维数不对。你换过来就行了。你的调用要是在for循环体中，那么你的输入参数比如x是否进行矩阵维数扩展，即表示成x(1,k)这种形式？（这条可能就是产生问题的最主要原因）另外x和u的维数是否正确？我看这个函数程序中的x应该是3维的列向量还有你函数中xp=zeros(3,size(x,2));这句所生成的是一个三维的列向量，那么下面的xp(1,1)..xp(3,1)这些调用是否合理等都需要认真考虑function[sys,x0,str,ts]=str_sim(t,x,z,flag,T,n,R1,xinit,Pinit)%EKFSExtendedKalmanfilterS-functionforrekursiveidentification%ThisM-fileisdesignedtobeusedinaSimulinkS-functionblock.%ItimplementsanextendedKalmanfilterforjointstateestimation%andparametertracking.Sofar,onlyoutputerrorSISOmodelsare%supported%%Input:theta%Output:(dummy)%switchflag,%%%%%%%%%%%%%%%%%%%Initialization%%%%%%%%%%%%%%%%%%%case0,%M鰆ligenkanjagkommap?n錱onsmartinitialiseringh鋜...x0=[xinit(:);Pinit(:)];%initthestatevectorts=[T,0];sys(1)=0;%0continuousstatessys(2)=length(x0);%enoughdiscretestatestoholdxandPsys(3)=3*n;%Allstatesasoutputssys(4)=2;%inputs:z=[yu]sys(5)=0;%0rootssys(6)=1;%Directfeedthroughsys(7)=1;%1sampletime%%%%%%%%%%%Update%%%%%%%%%%%case2,xs=x(1:3*n,1);P=zeros(3*n,3*n);P(:)=x(3*n+1:end,1);%Measurementupdatey=z(1);u=z(2);R=1;H=[1zeros(1,n-1+2*n)];K=P*H'/(H*P*H'+R);%onlySISOmodelssupportedxs=xs+K*(y-H*xs);%TimeupdateQ=eye(2*n)*R1;a=xs(n+1:2*n);A=[-a(:)[eye(n-1);zeros(1,n-1)]];F=[A-xs(1)*eye(n)u*eye(n);zeros(2*n,n)eye(2*n)];G=[zeros(n,2*n);eye(2*n)];P=F*(P-K*H*P)*F'+G*Q*G';xs(1:n)=[-xs(n+1:2*n)[eye(n-1);zeros(1,n-1)]]*xs(1:n)+xs(2*n+1:3*n)*u;sys=[xs(:);P(:)];%%%%%%%%%%%%Outputs%%%%%%%%%%%%case3,xs=x(1:3*n,1);P=zeros(3*n,3*n);P(:)=x(3*n+1:end,1);%Measurementupdatey=z(1);u=z(2);R=1;H=[1zeros(1,n-1+2*n)];K=P*H'/(H*P*H'+R);%onlySISOmodelssupportedxs=xs+K*(y-H*xs);sys=x(1:3*n,:);%%%%%%%%%%%%%%%%%%%%%%%%GetTimeOfNextVarHit%%%%%%%%%%%%%%%%%%%%%%%%case4,sys=[];%%%%%%%%%%%%%%%%%%%%%Unexpectedflags%%%%%%%%%%%%%%%%%%%%%otherwisesys=[];end%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;endUk=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);endtt=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');web-browser[YY,XX]=EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:)=0;forii=1:1:length(Ap)-1Ap(ii+1,ii)=1;endinx=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;%FIRSTSTEPendPPk=Ap*PPk*Ap';%SECONDSTEPKk=PPk*C'*inv((C*PPk*C')+(V*Qv*V'));%THIRDSTEPforii=1:1:length(Xint_v)XUPK(ii,1)=XKk(ii)^2+UUk(ii);%UPPEREQUATIONS.Zk(ii,1)=cos(XUPK(ii))+VVk(ii);%UPPEREQUATIONS.endforii=1:1:length(XKk)XBARk(ii,1)=XKk(ii)+Kk(ii)*(Zk(ii)-(cos(XKk(ii))));%FOURTHSTEPendII=eye(5,5);Pk=(II-Kk*C)*PPk;%FIFTHSTEP%--------------------------------------------------------------------------%--------------------------------------------------------------------------forii=1:1:nUUk=[Uk(ii+1);0;0;0;0];PPk=XBARk*XBARk';VVk=[Vk(ii+1);0;0;0;0];XKk=exp(-XBARk);%FIRSTSTEPPPkM=Ap*PPk*Ap';%SECONDSTEPKk=PPkM*C'