%********************************************************************%   
% The noise-approximations given to the Kalman filter, taken into covar.
% Fixed these measurements 24.05.01
S1_xB=1e-7;
S2_xB=[0.4 0; 0 0.0091/180*pi];
S1_yA=1e-7;
S2_yA=[0.5 0; 0 0.012/180*pi];
%********************************************************************%   



%S1_xB=1e-8;
%S2_xB=[0.015 0; 0 0.002/180*pi];
%S1_yA=1e-8;
%S2_yA=[0.03 0; 0 0.0014/180*pi];
Ts=0.03;

% State matrices in y-axes. Alfa-direction.

  AyA = [  0    1.0000         0         0
   	      0         0  700.0000         0
      	   0         0         0    1.0000
         	0         0         0  -34.6900  ];
      
	ByA = [      0
   	          0
      	       0
         	    3.51119 ];
          
	C   =  eye(4);
          
   D   =  zeros(4,1);
   
   xy0  = [ 0 0 0 0 ]';
   
% States not used are cut
	CyA = [ 1 0 0 0
           0 0 1 0 ];
%********************************************************************%   
      

% State matrices in x-axes. Beta-direction
           
   AxB = [  0    1.0500         0         0
   	      0         0 -700.0000         0
      	   0         0         0    1.1000
            0         0         0  -32.1800  ];
         
         
 	BxB = [      0
   	          0
      	       0
         	    3.7921 ];
          
	C   =  eye(4);
          
   D   =  zeros(4,1);
   
   xx0  = [ 0 0 0 0 ]';
   
   
% States not used are cut

	CxB = [ 1 0 0 0
		     0 0 1 0 ];
%********************************************************************%   

        
% MPC-Block. ss -> create state space. c2d -> convert from continous to discrete.
% This defines the state-matrices, one set in y-axes, one set in x-axes. This is the LTI-model
% loaded into the MPC-controller.

   mpc_sysc_y = ss(AyA,ByA,CyA,[0]);
   mpc_sysd_y = c2d(mpc_sysc_y,Ts);
   mpc_sysc_x = ss(AxB,BxB,CxB,[0]);
   mpc_sysd_x = c2d(mpc_sysc_x,Ts);
   
%********************************************************************%   

   % Constraints
   umin=-10;
   umax= 10;
   dumin=-Inf;
   dumax=Inf;
   ymin= [-30 , -15*pi/180];
   ymax= [ 30 ,  15*pi/180];
   
   limits={umin,umax,dumin,dumax,ymin,ymax};
   
   % Covariance used for error estimation in the Kalman-filter
   MPCadd_x.covmat = {S1_xB,S2_xB};
   MPCadd_y.covmat = {S1_yA,S2_yA};
   
   