/* -*- MaTX -*-
 *
 *  NAME
 *      ric() - Solution of continous-time Riccati equation
 *
 *  SYNOPSIS
 *      P = ric(A,Q,R)
 *         Matrix P;
 *         Matrix A,Q,R;
 *
 *      P = ric(A,Q,R,tol1)
 *         Matrix P;
 *         Matrix A,Q,R;
 *         Real tol1;
 *
 *      P = ric(A,Q,R,tol1,tol2)
 *         Matrix P;
 *         Matrix A,Q,R;
 *         Real tol1,tol2;
 *
 *  DESCRIPTION
 *      ric(A,R,Q) returns the stabilizing solution of the continuous-time 
 *      Riccati equation
 *
 *        A#*P + P*A - P*R*P + Q = 0
 *
 *      by using the arimoto and potter's method, where R is symmetric and 
 *      nonnegative definite and Q is symmetric.
 *
 *      If frobenius norm of the Riccati residual is greater than tol1,
 *      warning message is shown. Default tol1 is 1.0;
 *
 *      If there are jw-axis closed loop poles, warning message
 *      is shown.  ric(..., tol2) specifies the distance from the
 *      jw-axis. Default tol2 is 1.0E-6.
 *
 *  SEE ALSO
 *      are, riccati, and lqr
 */

Func Matrix ric(A, Q, R, tol1, tol2, ...)
	Matrix A;
	Matrix Q;
	Matrix R;
	Real tol1;
	Real tol2;
{
	Matrix P, H, U, V, T, Perr, pc;
	Integer i;
	Real res, tmp;

	error(nargchk(3, 5, nargs, "ric"));
	
	if (nargs < 4) {
		tol1 = 1.0;
	}
	if (nargs < 5) {
		tol2 = 1.0E-6;
	}

	H = [[ A, -R ]
		 [-Q, -A#]];

	T = eigvec(H);
	if (version() == 4) {
		U = T(0, 1, A);
		V = T(1, 1, A);
	} else {
		U = T(1, 2, A);
		V = T(2, 2, A);
	}
	P = V * U~;
	
	if (isreal(A) && isreal(Q) && isreal(R)) {
		P = Re(P);
		P = (P' + P)/2;
	} else {
		P = (P# + P)/2;
	}

	// Check redidual
	Perr = A#*P + P*A - P*R*P + Q;
	res = frobnorm(Perr);
	if (res > tol1) {
		warning("ric(): Frobenius norm of the Riccati residual: %g\n", res);
	}

	// Check jw-axis roots:
	pc = eigval(A - R*P);
	{tmp,i} = minimum(abs(Array(Re(pc))));
	if (tmp < tol2) {
		warning("ric(): There are jw-axis closed loop poles (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
		warning("       Results may be inaccurate !!\n");
	}

	// Check positive definiteness of P:
	pc = eigval(P);
	{tmp,i} = minimum(Re(pc));
	if (tmp < 0.0) {
		warning("ric(): P is not positive\n");
		warning("The smallest eigenvalue of P is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}
	
	return P;
}

Func Matrix Ric(A, Q, R, tol1, tol2, ...)
	Matrix A;
	Matrix Q;
	Matrix R;
	Real tol1;
	Real tol2;
{
	error(nargchk(3, 5, nargs, "Ric"));
	
	if (nargs == 3) {
		return ric(A, Q, R);
	} else if (nargs == 4) {
		return ric(A, Q, R, tol1);
	} else {
		return ric(A, Q, R, tol1, tol2);

	}
}