
printf( "Starting the lqr test...\n" );
rfile ode23

lqr = function( a, b, q, r ) 
{
  m = size(a)[1]; n = size(a)[2];
  mb = size(b)[1]; nb = size(b)[2];
  mq = size(q)[1]; nq = size(q)[2];
  
  if ( m != mq || n != nq ) 
  {
    fprintf( "stderr", "A and Q must be the same size.\n" );
    quit
  }
    
  mr = size(r)[1]; nr = size(r)[2];
  if ( mr != nr || nb != mr ) 
  {
    fprintf( "stderr", "B and R must be consistent.\n" );
    quit
  }
    
  nn = zeros( m, nb );
  
  // Start eigenvector decomposition by finding eigenvectors of Hamiltonian:
  
  e = eig( [ a, solve(r',b')'*b'; q, -a' ] );
  v = e.vec; d = e.val;
    
  index = sort( real( d ) ).ind;
  d = real( d[ index ] );
  
  if ( !( d[n] < 0 && d[n+1] > 0 ) ) 
  {
    fprintf( "stderr", "Can't order eigenvalues.\n" );
    quit
  }
    
  chi = v[ 1:n; index[1:n] ];
  lambda = v[ (n+1):(2*n); index[1:n] ];
  s = -real(solve(chi',lambda')');
  k = solve( r, nn'+b'*s );
  
  return << k=k; s=s >>;
  
};


// Now run a little test problem.

k = 1; m = 1; c = .1;
a = [    0,    1,    0,    0; -k/m, -c/m,  k/m,  c/m; 0,    0,    0,    1; k/m,  c/m, -k/m, -c/m ];
b = [ 0; 1/m; 0; 0 ];
qxx = diag( [0, 0, 100, 0] );
ruu = [1];

K = lqr( a, b, qxx, ruu ).k;

dot = function( t, x ) 
{
  global (a, b, K)
  return (a-b*K)*x + b*K*([1,0,1,0]')
};

x = ode( dot, 0, 15, [0,0,0,0]',.01, 1.e-5, 1.e-5);

m = maxi( x[;2] );

if ( (abs( x[m;2] - 1.195 ) > 0.001)  || ...
     any (abs( x[x.nr;2,4] - 1 ) > 0.001) ) {
     printf( "...failed.\a\n" );
else
     plgrid ();
     ptitle ("LQR Test Results");
     plot (x);
     printf( "...passed.\n" );
}
