%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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:         27.10.2010
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



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

dx_value = 1e-7;
du_value = 1e-7;

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

dx= zeros(4,1);
for i =1:4
    dx(i)=dx_value;
    x = x0 + dx;
    xdot0=v4_new_4d_model(0,x0,u0,'derivatives',par);
    xdot=v4_new_4d_model(0,x,u0,'derivatives',par);
    
    y0 = v4_new_4d_model(0,x0,u0,'measurements',par);
    y = v4_new_4d_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=v4_new_4d_model(0,x0,u0,'derivatives',par);
    xdot=v4_new_4d_model(0,x0,u,'derivatives',par);
   
    y = v4_new_4d_model(0,x0,u,'measurements',par);
    
    B(:,i) = (xdot-xdot0)/du(i);
    D(:,i) = (y-y0)/du(i);
    du(i)=0;
end
end