function [sys,x0,str,ts]=kalman(t,x,u,flag,k)
switch flag,
case 0 %初始化設(shè)置
[sys,x0,str,ts]=mdlInitializeSizes; 
case 3
sys=mdlOutputs(t,x,u,k);
case {1,2,4,9}
sys=[];
otherwise
error(['Unhandled flag=',num2str(flag)]);
end
%=====================================================
% mdlInitializeSizes進(jìn)行初始化,設(shè)置系統(tǒng)變量的大小
%=====================================================
function [sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=0;
sizes.NumOutputs=2;
sizes.NumInputs=3;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;
sys=simsizes(sizes);
x0=[0 0 0 0 0]';
str=[];
ts=[-1,0];
%=====================================================
% 當(dāng)flag=3時(shí),計(jì)算系統(tǒng)的輸出變量:返回三個狀態(tài)
%=====================================================
function sys=kalman(t,x,u,k)
R=1;
L=0.02;
M=-0.0067;
Ls=L-M;
% J=0.005;
k=0.382;
T=0.002;%T=0&x5-per=pi/3&x5-per=2*pi/3&x5-per=pi&x5-per=4*pi/3&x5-per=5*pi/3&x5-per<2*pi
q=5*pi/3;
a11=-2/3;a12=4/pi;
a21=-2/3;a22=-2/pi;
a31=4/3;a32=-2/pi;
end
A=[1-R*T/Ls 0 0 k*T*(a11+a12*(x5-q-per))/Ls 0;0 1-R*T/Ls 0 k*T*(a21+a22*(x5-q-per))/Ls0;...001-R*T/Ls k*T*(a31+a32*(x5-q-per))/Ls 0;0 0 0 1 0;0 0 0 T 1];
A1=Jacobian(A*X,X');
B=[T/(3*Ls) -T/(3*Ls) 0 0 0;0 T/(3*Ls) -T/(3*Ls) 0 0;...
-T/(3*Ls) 0 T/(3*Ls) 0 0]';
C=[1 0 0 0 0;0 1 0 0 0;0 0 1 0 0];
P=[0.01 0.01 0.01 10 1];
R=diag([0.02 0.02 0.02]);
Q=diag([0.04 0.04 0.04 0.05 0.02]);
Xhat1=A*x+B*u;
Phat1=A1*P*A1'+Q;
K=Phat1*C'*inv,[C*Phat1*C'+R];
xhat=[eye(5)-K*C]*(A*X+H*u)+K*X,[[1:3],;];
Phat=[eye(5)-K*C]*Phat1;
sys(1,1)=x4;
sys(2,1)=x5;