擴展卡爾曼濾波的S函數(shù)實現(xiàn)

1、擴展卡爾曼濾波器的s函數(shù)編寫 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);endfunction sys,x0,str,ts=mdlInitializeSizessizes = sims

2、izes;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_bas

3、al = 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;% for i=1:3% for j=1:3% P(i,j)=x(3*i+j);% end% endP=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_bas

4、al) - (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

5、(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% endfunction 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 agr

6、ee.哪里錯了？最佳答案 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)，這時又變成一個數(shù)了，后面xp=xp+K*(D-H);沒有問題。但是P=P-K*H*P;這里就出現(xiàn)了矩陣維數(shù)不對。你換過來就行了。你的調(diào)用要是在for循環(huán)體中，那么你的輸入?yún)?shù)比如x是否進行矩陣維數(shù)擴展，即表示成x(1,k)這種形式？（這條可能就是產(chǎn)生問題的最主要原因）另外x和u的維數(shù)是否正確？我看這個函數(shù)程序中的x應(yīng)該是3維的列向量還有你函數(shù)中xp=zeros(3,

7、size(x,2);這句所生成的是一個三維的列向量，那么下面的xp(1,1).xp(3,1)這些調(diào)用是否合理等都需要認(rèn)真考慮function sys,x0,str,ts = str_sim(t,x,z,flag,T,n,R1,xinit,Pinit)%EKFS Extended Kalman filter S-function for rekursive identification% This M-file is designed to be used in a Simulink S-function block.% It implements an extended Kalman filt

8、er for joint state estimation% and parameter tracking. Sofar, only output error SISO models are % supported% Input: theta% Output: (dummy)%switch flag, % % Initialization % % case 0, % M鰆ligen kan jag komma p?n錱on smart initialisering h鋜. x0 = xinit(:); Pinit(:); % init the state vector ts = T,0; sy

9、s(1) = 0;% 0 continuous states sys(2) = length(x0);% enough discrete states to hold x and P sys(3) = 3*n; % All states as outputs sys(4) = 2; % inputs: z = y u sys(5) = 0;% 0 roots sys(6) = 1;% Direct feedthrough sys(7) = 1;% 1 sample time % % Update % % case 2, xs = x(1:3*n,1); P = zeros(3*n,3*n);

10、P(:) = x(3*n+1:end,1); % Measurement update y = z(1); u = z(2); R = 1; H = 1 zeros(1,n-1+2*n); K = P*H'/(H*P*H'+R); % only SISO models supported xs = xs + K*(y-H*xs); % Time update Q = 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) ey

11、e(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 % % case 3, xs = x(1:3*n,1); P = zeros(3*n,3*n); P(:) = x(3*n+1:end,1); % Measurement update y = z(1); u = z(2); R = 1; H = 1 ze

12、ros(1,n-1+2*n); K = P*H'/(H*P*H'+R); % only SISO models supported xs = xs + K*(y-H*xs); sys = x(1:3*n,:); % % GetTimeOfNextVarHit % % case 4, sys = ; % % Unexpected flags % % otherwise sys = ;end% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER% FOR GAUSSIA

13、N AND LINEAR STOCHASTIC DIFFERENCE EQUATION.% By (R.C.R.C.R),SPLABS,MPL.% (17 JULY 2005).% Help by Aarthi Nadarajan is acknowledged.% (drawback of EKF is when nonlinearity is high, we can extend the% approximation taking additional terms in Taylor's series).clc; close all; clear all;Xint_v = 1;

14、0; 0; 0; 0;wk = 1 0 0 0 0;vk = 1 0 0 0 0;for ii = 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 = 1 0 0 0 0;n = 100;YY XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);for it = 1

15、:1:length(XX) MSE(it) = YY(it) - XX(it);endtt = 1:1:length(XX);figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL');subplot(212); plot(YY); title('ESTIMATED SIGNAL');figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated');fi

16、gure(3); plot(MSE.2); title('Mean square error');web -browser function YY,XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-1 Ap(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'for ii = 1:1

17、:length(Xint_v)XKk(ii,1) = Xint_v(ii)2; % FIRST STEPendPPk = Ap*PPk*Ap' % SECOND STEPKk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') ); % THIRD STEPfor ii = 1:1:length(Xint_v)XUPK(ii,1) = XKk(ii)2 + UUk(ii); % UPPER EQUATIONS.Zk(ii,1) = cos(XUPK(ii) + VVk(ii); % UPPER EQUATIONS.endfor ii = 1

18、:1:length(XKk)XBARk(ii,1) = XKk(ii) + Kk(ii)*(Zk(ii) - (cos(XKk(ii) ; % FOURTH STEPendII = eye(5,5);Pk = ( II - Kk*C)*PPk; % FIFTH STEP%-%-for ii = 1:1:nUUk = Uk(ii+1); 0; 0; 0; 0;PPk = XBARk*XBARk'VVk = Vk(ii+1); 0; 0; 0; 0;XKk = exp(-XBARk); % FIRST STEPPPkM = Ap*PPk*Ap' % SECOND STEPKk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') ); % THIRD STEPfor nn = 1:1:length(XBARk)XUPK(nn) = exp(-XKk(nn) + UUk(nn); % UPPER EQUATION