chap6_2_constrained_statespace.m

%chapter 6.2 done out. Does not include e(t) values
 
clear all
close


%Input and output transfer matrices
A = [ tf( [ 1 -1.8629 +0.8669 ], 1, -1, 'variable', 'z^-1' ) 0;
      0 tf( [ 1 -1.8695 0.8737 ], 1, -1, 'variable', 'z^-1' ) ];
B = [ tf( [ 0.042 -0.038 ], 1, -1, 'variable', 'z^-1' )...
      tf( [ 0.4758 -0.4559 ], 1, -1, 'variable', 'z^-1' );
      tf( [ 0.0582 -0.054 ], 1, -1, 'variable', 'z^-1' )...
      tf( [ 0.1445 -0.1361 ], 1, -1, 'variable', 'z^-1' )];

%constraints
p.dumax = [ 0.05 ; 0.05 ];
p.dumin = -p.dumax;

p.Umax = [ 0.3; 0.3 ];
p.Umin = [ -0.3; -0.3 ];

p.Ymax = [ 0.8; 0.5 ];
p.Ymin = [ 0; 0 ];
  
%control parameters
p.N1 = 1;
p.N2 = 3;
p.N3 = 2;
p.lmb = 0.05;
p.alpha = 0;  


p.n = size( A, 2 );
p.m = size( B, 2 );

na = getnab( A ) - 1;
nb = getnab( B ) - 1;

A_ = A*tf( [ 1 -1 ], 1, -1, 'variable', 'z^-1' );

%separate A_dash, B into separate z^-a matrices 
A_dash = eye( p.n ) - A_;
A_dash = A_dash*tf( [ 1 0 ], 1, -1, 'variable', 'z' );

%multiplying by z leaves 4th coefficient as 0. This routine ensures all
%values are only those necessary as other routines rely on the A.num size
for r = 1:size( A_dash, 1 )
    for c = 1:size( A_dash, 2 )
        while( A_dash.num{r,c}(end) == 0 && size( A_dash.num{r,c}, 2 ) > 1 )
            A_dash.num{r,c} = A_dash.num{r,c}( 1:end-1 );
        end
    end        
end


%get rid of bug zero at end of A_dash

A_dash_zsep = separate( A_dash );
B_zsep = separate( B );

%create M matrix
M = [ A_dash_zsep B_zsep( :, (p.m+1):end );
      eye( p.n*na ) zeros( na*p.n, p.n+p.m*nb  );
      zeros( p.m*nb, ( na+1 )*p.n ) [ zeros( p.m, p.m*nb ) ; eye( p.m*( nb-1 ) )...
      zeros( p.m*(nb-1), p.m ) ] ];
  
%create N matrix
N = [ B_zsep( :, 1:p.m ); zeros( na*p.n, p.m ); eye( p.m ); zeros( p.m*( nb-1 ), p.m ) ];

%create Q
Q = [ eye( p.n ) zeros( p.n, p.n*na+p.m*nb ) ];

%create P
P = [ eye( p.n ); zeros( na*p.n+p.m*nb, p.n ) ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%start rest from here%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%define state vector
%x(t) = [ y(t).' y(t-1).'... y(t-na).' du(t-1).' ...du(t-nb).' ]
x = zeros( p.n*(na+1)+p.m*nb, 1 );



%reference
ref = [ 0.5*ones( 1, 50 )  0.4*ones( 1, 50 );  0.3*ones( 1, 100 ) ];
Y = zeros( 2, size( ref, 2 ) );
du = zeros( p.m*p.N3, 1 );

FH = predMats( Q, M, N, p );
[ F H ] = FH{ 1, 1:2 };

%extract appropriate FN12, HN123 matrix according to p.N1 p.N2 p.N3 thresholds
FN12 = F( 1+(p.N1-1)*p.n:1+(p.N2-1)*p.n+p.n-1, 1:( na+1 )*p.n+p.m*nb );
HN123 = H( 1+(p.N1-1)*p.n:1+(p.N2-1)*p.n+p.n-1, 1:p.m*( p.N3-p.N1+1 )  );


%create R as desired
R = eye( size( HN123, 1 ) );
init = HN123.'*R*HN123;

%create Q_ as desired
Q_ = p.lmb*eye( size( init, 2 ) );

%constraints matrices
Rcon = formRss( p, HN123 );
options = optimset( 'Display','off','LargeScale','off' );

%no constraints optimization matrix
optMat = ( init + Q_ )^-1*HN123.'*R;

%initialise Uprev and Upast
Uprev = zeros( p.m, 1 );
Upast = Uprev;


for t = 1:size( ref, 2 )
    
    if( t+p.N2 <= size( ref, 2 ) )
        p.r = ref( :,t:t+p.N2 );
    else
       p.r = [ ref( :,t:end ) ref(:,end)*ones( 1, ( t+p.N2 - size( ref, 2 ) ) ) ];
    end
    
    Y( :, t ) = Q*x;   
    p.y = Y( :, t );
    
    w = futureSetPt( p ); 
    w = w( 1+( p.N1-1 )*p.n:1+( p.N2-1 )*p.n+p.n-1, 1);
    
    ccon = formcss( p, Uprev, FN12*x );
    
    %cost function and evaluation to find constrained du
    J = @(du)( ( HN123*du + FN12*x - w ).'*R*( HN123*du + FN12*x - w ) + du.'*Q_*du );
    du = fmincon( J, du, Rcon, ccon,[],[],[],[],[], options );
    
    %this gives optimum value of du
    %du = optMat*( w - FN12*x );

    x = M*x + N*du( 1:p.m, 1 );
    
    U( :, t ) = Uprev + du( 1:p.m, 1 ); 
    Uprev = U( :, t );    
   
end

subplot( 2, 1, 1 )
plot( Y( 1, : ) )
hold on 
plot( ref( 1, : ), 'r:' )
plot( Y( 2, : ),'r' )
plot( ref( 2, : ), ':' )

subplot( 2, 1, 2 )
plot( U( 1, : ) )
hold on 
plot( U( 2, : ), 'r' )