% 3x1 system
%
% MV=[1], MD=[2], UD=[3]
% Y=[1]

% Prepare parameters for mpc_ex1a

% True plant & true initial state
%********************************************************************%   
S1_xB=1e-7;
S2_xB=[0.015 0; 0 0.002/180*pi];
S1_yA=1e-7;
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.1119 ];
          
	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.0000         0         0
   	      0         0 -700.0000         0
      	   0         0         0    1.0000
            0         0         0  -33.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.

   sys_x = ss(AxB,BxB,CxB,[0]);
  sys_y = ss(AxA,BxA,CxA,[0]);
   
%********************************************************************%   

A=sys.a;
B=sys.b;
C=sys.c;
D=sys.d;

x0=[0 0 0 0]';


Ts=.03;     %Sample time
Tstop=30;  %Simulation time

% prediction model
model_x=c2d(sys_x,Ts);
model_y=c2d(sys_y,Ts);

p=10;
moves=3;
xhat0=0*x0;

MPCadd.mdindex=[];

MPCadd.covmat = {S1_xB,S2_xB};
MPCadd_y.covmat = {S1_yA,S2_yA};

   % 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};

weights={0,1,[0 1]};


% Reference and measured disturbance signals
time=0:Ts:Tstop;
trv=[time;
    sin(0:2*Ts:2*Tstop)
    zeros(size(0:Ts:Tstop))];
   %zeros(size(0:Ts:5)),ones(size(5+Ts:Ts:Tstop))
   %zeros(size(0:Ts:10)),ones(size(10+Ts:Ts:Tstop))];

save rv trv
    
open_system('mpc_ex1a.mdl')
sim('mpc_ex1a',Tstop)