扩展卡尔曼滤波的S函数实现试题.doc

扩展卡尔曼滤波器的s函数编写 function [sys,x0,str,ts] = sekfs(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 2, sys=mdlUpdate(t,x,u); case 3, sys=mdlOutputs(t,x,u); case {1,4,9}, sys=[]; otherwise error([Unhandled flag = ,num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 3; sizes.NumOutputs = 1; sizes.NumInputs = 1; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [4.5000 15.0000 15.0000]; str = []; ts = 1e-6; function sys=mdlUpdate(t,x,u) global Qon; Qon=1; R=1e-2; Q=Qon*diag([1e-2 1e-2 1e-2]); G_basal = 4.5; % mmol/L X_basal = 15; % mU/L I_basal = 15; % mU/L P1 = 0.028735; % min-1 P2 = 0.028344; % min-1 P3 = 5.035e-5; % mU/L V1 = 12; % L n = 5/54; % min D = 5; To=1e-6; % for i=1:3 % for j=1:3 % P(i,j)=x(3*i+j); % end % end P=[0.01 0 0; 0 0.01 0;0 0 0.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)]; for i=1:3 sys(i)=xp(i,1); end % for i=1:3 % for j=1:3 % sys(i,j)=P(3*i+j); % end % end function sys=mdlOutputs(t,x,u) sys =x(1); Error in untitled/S-Function while executing M-File S-function sekfs, flag = 2 (update), at time 0. MATLAB error message: Inner matrix dimensions must agree. 哪里错了？ 最佳答案 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)这些调用是否合理等都需要认真考虑 func