infrakalmanfilter.m

卡尔曼滤波算法源程序

function [sys,x0,str,ts] = infrakalmanfilter(t,x,u,flag,infraredsampletime,infraredinitstate,infraredinnum,infraredoutnum,infraredp0,infraredH,infraredB,infraredG,infraredC)

%SFUNTMPL General M-file S-function template
%   With M-file S-functions, you can define you own ordinary differential
%   equations (ODEs), discrete system equations, and/or just about
%   any type of algorithm to be used within a Simulink block diagram.
%
%   The general form of an M-File S-function syntax is:
%       [SYS,X0,STR,TS] = SFUNC(T,X,U,FLAG,P1,...,Pn)
%
%   What is returned by SFUNC at a given point in time, T, depends on the
%   value of the FLAG, the current state vector, X, and the current
%   input vector, U.
%
%   FLAG   RESULT             DESCRIPTION
%   -----  ------             --------------------------------------------
%   0      [SIZES,X0,STR,TS]  Initialization, return system sizes in SYS,
%                             initial state in X0, state ordering strings
%                             in STR, and sample times in TS.
%   1      DX                 Return continuous state derivatives in SYS.
%   2      DS                 Update discrete states SYS = X(n+1)
%   3      Y                  Return outputs in SYS.
%   4      TNEXT              Return next time hit for variable step sample
%                             time in SYS.
%   5                         Reserved for future (root finding).
%   9      []                 Termination, perform any cleanup SYS=[].
%
%
%   The state vectors, X and X0 consists of continuous states followed
%   by discrete states.
%
%   Optional parameters, P1,...,Pn can be provided to the S-function and
%   used during any FLAG operation.
%
%   When SFUNC is called with FLAG = 0, the following information
%   should be returned:
%
%      SYS(1) = Number of continuous states.
%      SYS(2) = Number of discrete states.
%      SYS(3) = Number of outputs.
%      SYS(4) = Number of inputs.
%               Any of the first four elements in SYS can be specified
%               as -1 indicating that they are dynamically sized. The
%               actual length for all other flags will be equal to the
%               length of the input, U.
%      SYS(5) = Reserved for root finding. Must be zero.
%      SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function
%               has direct feedthrough if U is used during the FLAG=3
%               call. Setting this to 0 is akin to making a promise that
%               U will not be used during FLAG=3. If you break the promise
%               then unpredictable results will occur.
%      SYS(7) = Number of sample times. This is the number of rows in TS.
%
%
%      X0     = Initial state conditions or [] if no states.
%
%      STR    = State ordering strings which is generally specified as [].
%
%      TS     = An m-by-2 matrix containing the sample time
%               (period, offset) information. Where m = number of sample
%               times. The ordering of the sample times must be:
%
%               TS = [0      0,      : Continuous sample time.
%                     0      1,      : Continuous, but fixed in minor step
%                                      sample time.
%                     PERIOD OFFSET, : Discrete sample time where
%                                      PERIOD > 0 & OFFSET < PERIOD.
%                     -2     0];     : Variable step discrete sample time
%                                      where FLAG=4 is used to get time of
%                                      next hit.
%
%               There can be more than one sample time providing
%               they are ordered such that they are monotonically
%               increasing. Only the needed sample times should be
%               specified in TS. When specifying than one
%               sample time, you must check for sample hits explicitly by
%               seeing if
%                  abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD)
%               is within a specified tolerance, generally 1e-8. This
%               tolerance is dependent upon your model's sampling times
%               and simulation time.
%
%               You can also specify that the sample time of the S-function
%               is inherited from the driving block. For functions which
%               change during minor steps, this is done by
%               specifying SYS(7) = 1 and TS = [-1 0]. For functions which
%               are held during minor steps, this is done by specifying
%               SYS(7) = 1 and TS = [-1 1].

%   Copyright 1990-2002 The MathWorks, Inc.
%   $Revision: 1.18 $

%
% The following outlines the general structure of an S-function.
%
switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes(infraredsampletime,infraredinitstate,infraredinnum,infraredoutnum,infraredp0);

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    sys=mdlDerivatives(t,x,u);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u,infraredinitstate,infraredH,infraredB,infraredG,infraredC);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u,infraredinitstate,infraredoutnum,infraredC);

  %%%%%%%%%%%%%%%%%%%%%%%
  % 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(infraredsampletime,infraredinitstate,infraredinnum,infraredoutnum,infraredp0)

%
% 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  = length(infraredinitstate)*(2*length(infraredinitstate)+1+length(infraredinitstate)/2);%大小由状态向量的维数决定
sizes.NumOutputs     = infraredoutnum;
sizes.NumInputs      = infraredinnum;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
%dx(1)  =initstate ;%x[n|n-1]
%dx(2)  =0;        %n+1的先验状态估计P[n|n-1]
%dx(3)  = p0;           %n的更新状态估计值P[n|n]
%dx(4)  =0;             %n修正矩阵K[n]

len=length(infraredinitstate);
%状态初始值赋值
for i=1:1:len   
    x0(i)=infraredinitstate(i);
end
%更新状态误差矩阵P[n|n]
for i=1:1:len  
    for j=1:1:len
        x0(i*len+j)=infraredp0(i,j);
    end
end

%先验误差矩阵和修正矩阵
for i=len*(len+1)+1:1:len*(2.5*len+1)
    x0(i)=0;
end

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = infraredsampletime;

% 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,infraredinitstate,infraredH,infraredB,infraredG,infraredC)
len=length(infraredinitstate);
%取出状态向量
for i=1:1:len
    Xstate(i)=x(i);
end
Xstate=Xstate';

%取出更新状态误差矩阵
for i=1:1:len
    for j=1:1:len
        Pb(i,j)=x(i*len+j);
    end
end


ulen=length(u);
alen=(ulen-2)/2;%加速度向量长度
for i=1:1:alen
    acceleration(i)=u(alen+i);
end
acceleration=acceleration';

for i=1:1:alen
    Y(i)=u(i);
end
Y=Y';

Xstate=infraredH*Xstate+infraredB*acceleration;           %u(2)表示输入向量u

Pa= infraredH*Pb*infraredH' + infraredG*u(ulen-1)*infraredG'; %表示先验状态估计值
K= Pa*infraredC'/(infraredC*Pa*infraredC'+eye(alen)*u(ulen));      %增益因子
Pb= (eye(len)-K*infraredC)*Pa;                    %更新状态误差矩阵

Xstate=Xstate + K*(Y-infraredC*Xstate);           %计算出Xstate的估计状态

%把估计状态向量存入到X
for i=1:1:len   
    dx(i)=Xstate(i);
end

%把更新状态误差矩阵存入到X
for i=1:1:len  
    for j=1:1:len
        dx(i*len+j)=Pb(i,j);
    end
end
%把先验状态误差矩阵存入到X
for i=1:1:len  
    for j=1:1:len
        dx((i-1)*len+j+len*(len+1))=Pb(i,j);
    end
end
%把增益矩阵存入到X
for i=1:1:len  
    for j=1:1:alen
        dx((i-1)*alen+j+len*(2*len+1))=K(i,j);
    end
end
sys =dx;

% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u,infraredinitstate,infraredoutnum,infraredC)
len=length(infraredinitstate);
ylen=len/2;

%估计状态向量
for i=1:1:len
    Xstate(i)=x(i);
end

Y=infraredC*Xstate';                         %y
for i=1:1:ylen
    dy(i)=Y(i);
end

%更新误差矩阵
for i=1:1:len
    for j=1:1:len
        dy((i-1)*len+j+ylen)=x(i*len+j);
    end
end
%估计状态
for i=1:len
    dy(i+len*len+ylen)=Xstate(i);
end
%增益矩阵
%for i=1:1:len  
 %   for j=1:1:ylen
 %       dy((i-1)*ylen+j+ylen+len*len)=x((i-1)*ylen+j+len*(2*len+1));
  %  end
%end

sys =dy;

% end mdlOutputs

function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate