卡尔曼滤波程序
各位高手看看我这个程序有什么问题转速估计就是不对 ！谢谢
function = sfuntmpl(t,x,u,flag,T)
%SFUNTMPL General M-file S-function template
% The following outlines the general structure of an S-function.
%
global Q R x_1 P_1 delta P h Y out;
%initial parameters and states
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0,
=mdlInitializeSizes;
%%%%%%%%%%%%%%%
% Derivatives %
%%%%%%%%%%%%%%%
%case 1,
%sys=mdlDerivatives(t,x,u);
%%%%%%%%%%
% Update %
%%%%%%%%%%
case 2,
sys=mdlUpdate(t,x,u,T);
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
case 3,
sys=mdlOutputs(t,x,u);
%%%%%%%%%%%%%%%%%%%%%%%
% GetTimeOfNextVarHit %
%%%%%%%%%%%%%%%%%%%%%%%
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9,
sys=mdlTerminate(t,x,u);
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
% end sfuntmpl
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function =mdlInitializeSizes
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates= 0;
sizes.NumDiscStates= 5;
sizes.NumOutputs = 5;
sizes.NumInputs = 4;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0=zeros(5,1);K=zeros(5,2);
%
% str is always an empty matrix
%
str = [];
ts=;
%
% initialize the array of sample times
%
% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
%function sys=mdlDerivatives(t,x,u)
%sys = [];
% end mdlDerivatives
%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u,T)
Lr=0.127145;Ls=0.127145;Lm=0.1241;Rs=0.7384;Rr=0.7403;delta=1-Lm*Lm/(Ls*Lr);
P=;
Q=;
R=;
U=;
Y=;
%Prediction of state
A=[1-T*(Rs*Lr*Lr+Rr*Lm*Lm)/(delta*Ls*Lr*Lr),0,T*Lm*Rr/(delta*Ls*Lr*Lr),2*x(5)*T*Lm/(delta*Ls*Lr),0;
0,1-T*(Rs*Lr*Lr+Rr*Lm*Lm)/(delta*Ls*Lr*Lr),-2*x(5)*T*Lm/(delta*Ls*Lr),T*Lm*Rr/(delta*Ls*Lr*Lr),0;
T*Lm*Rr/Lr,0,1-T*Rr/Lr,-T*x(5)*2,0;
0,T*Lm*Rr/Lr,T*x(5)*2,1-T*Rr/Lr,0;
0,0,0,0,1]; %状态矩阵
x_1=[A(1,1)*x(1)+A(1,3)*x(3)+A(1,4)*x(4);
A(2,2)*x(2)+A(2,3)*x(3)+A(2,4)*x(4);
A(3,1)*x(1)+A(3,3)*x(3)+A(3,4)*x(4);
A(4,2)*x(2)+A(4,3)*x(3)+A(4,4)*x(4);
x(5)]+T*;%状态预测
diff_F=[1-T*(Rs*Lr*Lr+Rr*Lm*Lm)/(delta*Ls*Lr*Lr),0,T*Lm*Rr/(delta*Ls*Lr*Lr),2*x_1(5)*T*Lm/(delta*Ls*Lr),x_1(4)*T*Lm/(delta*Ls*Lr);
0,1-T*(Rs*Lr*Lr+Rr*Lm*Lm)/(delta*Ls*Lr*Lr),-2*x_1(5)*T*Lm/(delta*Ls*Lr),T*Lm*Rr/(delta*Ls*Lr*Lr),x_1(3)*T*Lm/(delta*Ls*Lr);
T*Lm*Rr/Lr,0,1-T*Rr/Lr,-T*x_1(5)*2,T*x_1(4);
0,T*Lm*Rr/Lr,T*x_1(5)*2,1-T*Rr/Lr,T*x_1(3);
0,0,0,0,1];
%Estimation of error covariance matrix
P_1=diff_F*P*diff_F'+Q;
%Calculation of h and diff_h
h=;
diff_h=;
%Computation of Kalman Filter
K=P_1*diff_h'*inv(diff_h*P_1*diff_h'+R);
%State estimetion
out=x_1+K*(Y-h);
sys=out;
%Updata of the error covariance matrix
P=P_1-K*diff_h*P_1;
end
% end mdlUpdate
%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u)
sys = x;
% end mdlOutputs
%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)
sys = [];
% end mdlTerminate
页:
[1]