卡尔曼滤波程序

tyler

各位高手看看我这个程序有什么问题
转速估计就是不对 ！谢谢
function [sys,x0,str,ts] = 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,
    [sys,x0,str,ts]=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 [sys,x0,str,ts]=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=[0.0001 0];
%
% 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=[10 0 0 0 0;0 10 0 0 0;0 0 10 0 0;0 0 0 10 0;0 0 0 0 0.11];
Q=[1.5e-7 0 0 0 0;0 1.5e-7 0 0 0;0 0 1.5e-8 0 0;0 0 0 1.5e-8 0;0 0 0 0 1.5e-9];
R=[0.0002 0;0 0.0002];
U=[u(1);u(2)];
Y=[u(3);u(4)];
        %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*[u(1)/(delta*Ls);u(2)/(delta*Ls);0;0;0];%状态预测
        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=[x_1(1);x_1(2)];
        diff_h=[1 0 0 0 0;0 1 0 0 0];
        %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