function [sys,x0,str,ts] = sfundsc1(t,x,u,flag,sysd)
%SFUNDSC1 Example memory M-file S-function with inherited sample time
%   This M-file S-function is an example of how to implement an
%   inherited sample time S-function which has state. The actual sample
%   time will be determined by what is driving this S-function. It may
%   be continuous or discrete. This S-function uses one discrete state 
%   element as storage such that the previous input is provided at the 
%   output.
%   
%   See sfuntmpl.m for a general S-function template.
%
%   See also SFUNTMPL.
    
%   Copyright 1990-2007 The MathWorks, Inc.
%   $Revision: 1.14.2.1 $

persistent A B C Q R n

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
   [sys,x0,str,ts,A,B,C,Q,R,n]=mdlInitializeSizes(sysd);
  
  %%%%%%%%%%  
  % Update %
  %%%%%%%%%%
  case 2,                                               
    sys = mdlUpdate(t,x,u,A,B,C,Q,R,n);
    
  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                               
    sys = mdlOutputs(t,x,u,C);    

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,                                               
    sys = [];

  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end

%end sfundsc1

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts,A,B,C,Q,R,n]=mdlInitializeSizes(sysd)

A=sysd.a;
B=sysd.b;
C=sysd.c;
n=length(A);

Q=sysd.Userdata.Q;
R=sysd.Userdata.R;

sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = n+n^2;
sizes.NumOutputs     = n;
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;

sys = simsizes(sizes);

x0  = zeros(n+n^2,1);
str = [];
ts  = [sysd.Ts 0]; % Inherited sample time



return

% end mdlInitializeSizes

%
%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=======================================================================

function sys = mdlUpdate(t,x,u,A,B,C,Q,R,n)


x_act=x(1:n);
y=u(1);
u_=u(2);

k=n+1;
for i=1:n
    for j=1:n
        P(i,j)=x(k);
        k=k+1;
    end
end


% predizione
x_=A*x_act+B*u_;
P_=A*P*A'+Q;

% correzione
K=P_*C'*inv(C*P_*C'+R);
x_est=x_+K*(y-C*x_);
P=(eye(n)-K*C)*P_;


sys=x_est';
for i=1:n
    for j=1:n
        sys=[sys P(i,j)];
    end
end

return




%end mdlUpdate

%
%=======================================================================
% mdlOutputs
% Return the output vector for the S-function
%=======================================================================
%
function sys = mdlOutputs(t,x,u,C)
n=length(C);
sys = x(1:n);
return

%end mdlOutputs


