%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function that calculates the jackobians numerically with non constant 
% Alpha_L1_av for the new model. Output of the function is the numerical 
% linearized state space representation. 
% 
% A simple first order forward difference is used to approximate the 
% derivative. The stepsize are given below.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by:  Knut Åge Meland
% Date:         6.11.2010
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



function [A, B, C, D] = nummod_DiMeglio(x0,y0,u0,par)

dx_value = 1e-4;
du_value = 1e-4;

A = zeros(3,3);
B = zeros(3,3);
C = zeros(3,3);
D = zeros(3,3);

dx= zeros(3,1);
for i =1:3
    dx(i)=dx_value;
    x = x0 + dx;
    xdot0=v2_DiMeglio_model(0,x0,u0,'derivatives',par);
    xdot=v2_DiMeglio_model(0,x,u0,'derivatives',par);
    
    y0 = v2_DiMeglio_model(0,x0,u0,'measurements',par);
    y = v2_DiMeglio_model(0,x,u0,'measurements',par);

    A(:,i) = (xdot-xdot0)/dx(i);
    C(:,i) = (y-y0)/dx(i);
    dx(i)=0;
end

du=zeros(3,1);
for i =1:3
    du(i)=du_value;
    u = u0 + du;
    
    xdot0=v2_DiMeglio_model(0,x0,u0,'derivatives',par);
    xdot=v2_DiMeglio_model(0,x0,u,'derivatives',par);
   
    y = v2_DiMeglio_model(0,x0,u,'measurements',par);
    
    B(:,i) = (xdot-xdot0)/du(i);
    D(:,i) = (y-y0)/du(i);
    du(i)=0;
end
end