/* -*- MaTX -*-
 *
 *  NAME
 *      dlqr() - Discrete-time linear quadratic regulator 
 *
 *  SYNOPSIS
 *      {F,P} = dlqr(A,B,Q,R)
 *         Matrix F,P;
 *         Matrix A,B,Q,R;
 *
 *      {F,P} = dlqr(A,B,Q,R,S)
 *         Matrix F,P;
 *         Matrix A,B,Q,R,S;
 *
 *  DESCRIPTION
 *       For the linear system:
 *		
 *      	x[n+1] = Ax[n] + Bu[n] 
 *                
 *       dlqr() returns the optimal feedback gain matrix F such that 
 *       the feedback law  u = -Fx  minimizes the quadratic cost function:
 *
 *      	J = Sum (x#Qx + u#Ru)
 *
 *       dlqr() also returns the solution P of the Riccati equation:
 *
 *      	P - A#PA + A#PB(R + B#PB)~BP#A - Q = 0
 *
 *       dlqr(,,,.S) uses S to specify the cross-term that relates u to 
 *       x in the cost functional as
 *
 *          J = Sum (x#Qx + u#Ru + 2 x#Su) dt
 *
 *      DOptimal() is obsolete.  Use  {F} = dlqr(A,B,Q,R); F = -F;
 *
 *  SEE ALSO
 *      lqr and dlqe
 */

Func List dlqr(A,B,Q,R,S, ...)
	Matrix A,B,Q,R,S;
{
	Integer n;
	Matrix P,F,Dr;
	CoMatrix D,V;
	Index idx;
	String msg;

	error(nargchk(4, 5, nargs, "dlqr"));

	if (length((msg = abcdchk(A, B))) > 0) {
		error("dlqr(): " + msg);
	}

	if (nargs == 4) {
		S = Z(B);
	}

	n = Rows(A);

	if (n != Rows(Q) || n != Cols(Q)) { 
		error("dlqr(): A and Q must be the same size.\n");
	}
	if (Rows(R) != Cols(R) || Cols(B) != Rows(R)) {
		error("dlqr(): Dimension of B and R must be consistent.\n");
	}

	if (any(Re(eigval(Q)) .< -EPS) || norm(Q# -Q,1)/norm(Q,1) > EPS) {
		error("dlqr(): Q must be symmetric and positive semi-definite.\n");
	}

	if (any(Re(eigval(R)) .<= -EPS) || norm(R# -R,1)/norm(R,1) > EPS) {
		error("dlqr(): R must be symmetric and positive definite.\n");
	}

	{D,V} = eig([[A+B/R*B#/A#*Q, -B/R*B#/A#]
				  [    -A#\Q,       (A)~#   ]]);
	D = diag2vec(D);

	// Sort on magnitude of eigenvalues
	{Dr,idx} = sort(abs(D));
	if ( ! (Dr(n) < 1.0 && Dr(n+1) > 1.0) ) {
		error("dlqr(): (A,B) may be uncontrollable.\n");
	}

	// Select eigenvectors corresponding to the eigenvalues inside unit circle
	P = Re(V(n+1:2*n,idx(1:n))/V(1:n,idx(1:n)));
	F = (R + B#*P*B) \ (B#*P*A + S#);
	return {F, P};
}

Func Matrix DOptimal(A, B, Q, R, S, ...)
	Matrix A, B, Q, R, S;
{
	Matrix F;

	error(nargchk(4, 5, nargs, "DOptimal"));

	if (nargs == 4) {
		{F} = dlqr(A, B, Q, R);
		F = - F;
	} else if (nargs == 5) {
		{F} = dlqr(A, B, Q, R, S);
		F = - F;
	}

	return F;
}