/* -*- MaTX -*-
 *
 *  NAME
 *      lqrs() - Continuous-time linear quadratic regulator (Schur algorithm)
 *
 *  SYNOPSIS
 *      {F,P} = lqrs(A,B,Q,R)
 *          Matrix F,P;
 *          Matrix A,B,Q,R
 *
 *      {F,P} = lqrs(A,B,Q,R,S)
 *          Matrix F,P;
 *          Matrix A,B,Q,R
 *          Matrix S;
 *
 *  DESCRIPTION
 *       For the linear system:
 *          .
 *          x = Ax + Bu 
 *
 *       lqrs() 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
 *                                          
 *       lqrs() also returns the solution P of the Riccati equation:
 *
 *         P A + A# P - P B R~ B# P + Q = 0
 *
 *       lqrs(,...S) uses S to specify the the cross-term that relates 
 *       u to x in the cost functional as
 *
 *          J = Integral (x#Qx + u#Ru + 2 x#Su) dt
 *
 *       lqrs() uses the Schur algorithm and is more numerically
 *       reliable than lqr().
 *
 *  SEE ALSO
 *      lqr, lqry, and lqe
 */

Func List lqrs(A,B,Q,R,S, ...)
	Matrix A,B,Q,R,S;
{
	Matrix F,P,Ri;
	String msg;

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

	if (length((msg = abcdchk(A, B))) > 0) {
		error("lqrs(): " + msg);
	}
	
	if (nargs == 4) {
		Ri = R~;
		P = are(A, B*Ri*B#, Q);
		F = Ri*B#*P;
	} else {
		Ri = R~;
		P = are(A-B*Ri*S#, B*Ri*B#, Q-S*Ri*S#);
		F = Ri*(S# + B#*P);
	}

	return {F,P};
}