/* -*- MaTX -*-
 *
 *  NAME
 *      lqr() - Continuous-time linear quadratic regulator
 *
 *  SYNOPSIS
 *     {F,P} = lqr(A,B,Q,R)
 *        Matrix F,P;
 *        Matrix A,B,Q,R;
 *
 *     {F,P} = lqr(A,B,Q,R,S)
 *        Matrix F,P;
 *        Matrix A,B,Q,R;
 *        Matrix S;
 *
 *  DESCRIPTION
 *       For the linear system:
 *          .
 *          x = Ax + Bu 
 *                
 *       lqr(A,B,Q,R) calculates the optimal feedback gain matrix F such
 *       that the feedback law  u = -Fx  minimizes the cost function:
 *
 *          J = Integral (x#Qx + u#Ru) dt
 *
 *       lqr() also return the solution P of the Riccati equation:
 *
 *          P A + A# P - P B R~ B# P + Q = 0
 *
 *       lqr(...,S) uses S to specify the corss-term that relates u to 
 *       x in the cost functional as
 *
 *          J = Integral (x#Qx + u#Ru + 2 x#Su) dt
 *
 *       Optimal() is obsolete.  Use  {F} = lqr(A,B,Q,R);  F = -F;
 *
 *  SEE ALSO
 *      dlqr and lqe
 */

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

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

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

	n = Rows(A_);

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

	if (nargs == 5) {
		if (Rows(S) != n || Cols(S) != Cols(R)) {
			error("lqr(): Dimension of S must be consistent with Q and R.\n");
		}
		Q = Q_ - S/R*S#;
		A = A_ - B/R*S#;
	} else {
		S = Z(n,Cols(B));
		A = A_;
		Q = Q_;
	}

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

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

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

	// Sort on real part of eigenvalues
	{Dr,idx} = sort(Re(D));
	if (! (Dr(n) < 0.0 && 0.0 < Dr(n+1))) {
		error("lqr(): (A,B) may be uncontrollable.\n");
	}

	// Select eigenvectors corresponding to the negative eigenvalues
	P = -Re(V(n+1:2*n,idx(1:n))/V(1:n,idx(1:n)));
	F = R \ (S# + B#*P);

	return {F,P};
}

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

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

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