function [A,B,C,D,E,F,ys]=linearize(func,t0,x0,flag,u0,d0)

%
% cola_linearize - This function linearizes xprime=f(t,x,flag,u,d);
%
%    Written by: Atle C. Christiansen 
%    Modified by: Truls Larsson change of syntax
%    Modified by: Marius S. Govatsmark included measurements (y=Cx+Du,ys)
%                                                       06.04.1999
%     
%     SYNTAX: [A,B,C,D,E,F,ys]=linearize('func',t0,Xinit,flag,Uinit,d);
%
%     INPUTS:
%     func  - nonlinear model; must be on the form: [f]=func(t,x,flag,u,d)
%     t     - time
%     x     - initial values for the states
%     flag  - Just to obtain the same function form as ode15s ..
%     u     - inputs
%     d     - disturbances
%     
%
%     OUTPUTS:
%     The outputs are the matrixes from the linear model:
%
%     xprime = A*x + B*u +E*d
%     y      = C*x + D*u +F*d    
%
%     x - states
%     u - inputs             
%     d - disturbances  (optional)
%     y - measurements
%
%     and the steadystate values of the measurements
%
%     ys- measurements - steadystate value
%
%     GLOBAL VARIABLES
%     Y - vector with measurements 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%measurements - global variables
global Y data

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%dxdt=Ax+Bu
%y=Cx+Du
if nargin==5

%steadystate of measurements 
    [fs]=feval(func,t0,x0',u0);
    if abs(norm(fs))<1e-4 
       ys=Y;
    else
%        disp('You try to linearize around a point which is not in steadystate');
%        disp(norm(fs));
%       pause;
    end   

%%%%%%%%%%%%%%%%%%%%%%%%%%%    
  n=max(size(x0));
  r=max(size(u0));
  delta=1e-6;

  for i=1:n
    xh=x0; xh(i)=xh(i)+delta;
    xl=x0; xl(i)=xl(i)-delta;
    [fh]=feval(func,t0,xh',u0);
    yh=Y;
    [fl]=feval(func,t0,xl',u0);
    yl=Y;
    A1(:,i)=(fh(:)-fl(:))/(2*delta);
    C1(:,i)=(yh(:)-yl(:))/(2*delta);
  end

  for i=1:r
    uh=u0; uh(i)=uh(i)+delta;
    ul=u0; ul(i)=ul(i)-delta;
    [fh]=feval(func,t0,x0',uh);
    yh=Y;
    [fl]=feval(func,t0,x0',ul);
    yl=Y;
    B1(:,i)=(fh(:)-fl(:))/(2*delta);
    D1(:,i)=(yh(:)-yl(:))/(2*delta);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  delta=delta/2;

  for i=1:n
    xh=x0; xh(i)=xh(i)+delta;
    xl=x0; xl(i)=xl(i)-delta;
    [fh]=feval(func,t0,xh',u0);
    yh=Y;
    [fl]=feval(func,t0,xl',u0);
    yl=Y;
    A(:,i)=(fh(:)-fl(:))/(2*delta);
    C(:,i)=(yh(:)-yl(:))/(2*delta);
  end


  for i=1:r
    uh=u0; uh(i)=uh(i)+delta;
    ul=u0; ul(i)=ul(i)-delta;
    [fh]=feval(func,t0,x0',uh);
    yh=Y;
    [fl]=feval(func,t0,x0',ul);
    yl=Y;
    B(:,i)=(fh(:)-fl(:))/(2*delta);
    D(:,i)=(yh(:)-yl(:))/(2*delta);
  end

  A=(4*A-A1)/3;
  B=(4*B-B1)/3;
  C=(4*C-C1)/3;
  D=(4*D-D1)/3;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif nargin>=6
%dxdt=Ax+Bu+Ed
%y=Cx+Du+Fd
%disp('Hi')

%steadystate of measurements

    [fs]=feval(func,t0,x0,u0,d0);
    norm(fs);
    if abs(norm(fs))<1e-1
       ys=Y;
    else
       %disp('You try to linearize around a point which is not in steadystate');
       %pause;
       ys=Y;
    end   
       
%%%%%%%%%%%%%%%%%%%%%%%%%%  
  n=max(size(x0));
  r=max(size(u0));
  q=length(d0);
  delta=1e-6;

  for i=1:n
    xh=x0; xh(i)=xh(i)+delta;
    xl=x0; xl(i)=xl(i)-delta;
    [fh]=feval(func,t0,xh',u0,d0);
    yh=Y;
    [fl]=feval(func,t0,xl',u0,d0);
    yl=Y;
    A1(:,i)=(fh(:)-fl(:))/(2*delta);
    C1(:,i)=(yh(:)-yl(:))/(2*delta);
  end

  for i=1:r
    uh=u0; uh(i)=uh(i)+delta;
    ul=u0; ul(i)=ul(i)-delta;
    [fh]=feval(func,t0,x0',uh,d0);
    yh=Y;
    [fl]=feval(func,t0,x0',ul,d0);
    yl=Y;
    B1(:,i)=(fh(:)-fl(:))/(2*delta);
    D1(:,i)=(yh(:)-yl(:))/(2*delta);  
  end

  for i=1:q
    dh=d0; dh(i)=dh(i)+delta;
    dl=d0; dl(i)=dl(i)-delta;
    [fh]=feval(func,t0,x0',u0,dh);
    yh=Y;
    [fl]=feval(func,t0,x0',u0,dl);
    yl=Y;
    E1(:,i)=(fh(:)-fl(:))/(2*delta);
    F1(:,i)=(yh(:)-yl(:))/(2*delta);
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  delta=delta/2;
  for i=1:n
    xh=x0; xh(i)=xh(i)+delta;
    xl=x0; xl(i)=xl(i)-delta;
    [fh]=feval(func,t0,xh',u0,d0);
    yh=Y;
    [fl]=feval(func,t0,xl',u0,d0);
    yl=Y;
    A(:,i)=(fh(:)-fl(:))/(2*delta);
    C(:,i)=(yh(:)-yl(:))/(2*delta);  
  end

  for i=1:r
    uh=u0; uh(i)=uh(i)+delta;
    ul=u0; ul(i)=ul(i)-delta;
    [fh]=feval(func,t0,x0',uh,d0);
    yh=Y;
    [fl]=feval(func,t0,x0',ul,d0);
    yl=Y;
    B(:,i)=(fh(:)-fl(:))/(2*delta);
    D(:,i)=(yh(:)-yl(:))/(2*delta);    
  end

  for i=1:q
    dh=d0; dh(i)=dh(i)+delta;
    dl=d0; dl(i)=dl(i)-delta;
    [fh]=feval(func,t0,x0',u0,dh);
    yh=Y;
    [fl]=feval(func,t0,x0',u0,dl);
    yl=Y;
    E(:,i)=(fh(:)-fl(:))/(2*delta);
    F(:,i)=(yh(:)-yl(:))/(2*delta);      
  end

  A=(4*A-A1)/3;
  B=(4*B-B1)/3;
  C=(4*C-C1)/3;
  D=(4*D-D1)/3;
  E=(4*E-E1)/3;
  F=(4*F-F1)/3;
else 
  disp('Usage: [A,B,C,D,E,F]=linearize(func,t0,x0,u0,d0);')
  return
end













