/* -*- MaTX -*-
 *
 *  NAME
 *      lqry() - Continuous-time LQR with output weighting
 *
 *  SYNOPSIS
 *      {F,P} = lqry(A,B,C,D,Q,R)
 *         Matrix F,P;
 *         Matrix A,B,C,D,Q,R;
 *
 *      {F,P} = lqry(A,B,C,D,Q,R,S)
 *         Matrix F,P;
 *         Matrix A,B,C,D,Q,R;
 *         Matrix S;
 *
 *  DESCRIPTION
 *      For the linear system:
 *          .
 *          x = Ax + Bu
 *          y = Cx + Du
 *                
 *      lqry() calculates the optimal feedback gain matrix F such
 *      that the feedback law  u = -Fy  minimizes the cost function:
 *
 *          J = Integral (y#Qy + u#Ru) dt
 *
 *      lqry() also returns the solution P of the Riccati equation:
 *
 *         P A + A# P - P B R~ B# P + Q = 0
 *
 *       lqry(...,S) uses S to specify the corss-term that relates u to 
 *       y in the cost functional as
 *
 *          J = Integral (y#Qy + u#Ru + 2 y#Su) dt
 *
 *  SEE AlSO
 *      lqr and lqrs
 */

Func List lqry(A,B,C,D,Q,R,S, ...)
	Matrix A,B,C,D,Q,R,S;
{
	Matrix F,P;
	String msg;

	error(nargchk(6, 7, nargs, "lqry"));

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

	if (nargs == 6) {
		{F,P} = lqr(A, B, C#*Q*C, R+D#*Q*D, C#*Q*D);
	} else {
		{F,P} = lqr(A, B, C#*Q*C, R+D#*S+S#*D+D#*Q*D, C#*Q*D+C#*S);
	}

	return {F,P};
}