/* -*- MaTX -*-
 *
 *  NAME
 *      dric() - Solution of discrete-time Riccati equation
 *
 *  SYNOPSIS
 *      P = dric(A,B,Q,R)
 *         Matrix P;
 *         Matrix A,B,Q,R;
 *
 *      P = dric(A,B,Q,R,tol1)
 *         Matrix P;
 *         Matrix A,B,Q,R;
 *         Real tol1;
 *
 *      P = dric(A,B,Q,R,tol1,tol2)
 *         Matrix P;
 *         Matrix A,B,Q,R;
 *         Real tol1,tol2;
 *
 *  DESCRIPTION
 *      dric(A,B,Q,R) returns the stabilizing solution of discrete-time 
 *      Riccati equation
 *                                  
 *         P - A#PA + A#PB(R + B'PB)~ B'PA = Q
 *
 *      using generalized stable eigen vectors satisfying:
 *      
 *         [ A  O] [u] = lambda [I BR'B'] [u]
 *         [-Q  I] [v]          [O   A' ] [v]
 *
 *      If frobenius norm of the Riccati residual is greater than tol1,
 *      warning message is shown. Default tol1 is 1.0;
 *
 *      If there are unit-circle closed loop poles, warning message
 *      is shown.  dric(..., tol2) specifies the distance from the
 *      unit-circle. Default tol2 is 1.0E-6.
 *
 *  SEE ALSO
 *      ric and riccati
 */

Func Matrix dric(A, B, Q, R, tol1, tol2, ...)
	Matrix A, B, Q, R;
	Real tol1, tol2;
{
	Matrix P, HL, HR, EV, T, U, V, Perr, F, pc;
	Integer i, n, m;
	Index idx;
	Real res, tmp;

	error(nargchk(3, 5, nargs, "dric"));

	if (nargs < 4) {
		tol1 = 1.0;
	}
	if (nargs < 5) {
		tol2 = 1.0E-6;
	}

	n = Rows(A);
	m = Cols(B);

	HL = [[ A, Z(n)]
		  [-Q, I(n)]];

	HR = [[I(n), B*R~*B#]
		  [Z(n),   A#   ]];

	{EV, T} = eig(HL, HR);
	EV = diag2vec(EV);

	// Select finite eigenvalues and corresponding eigenvectors
	idx = find(isfinite(EV));
	EV = EV(idx,1);
	T = T(:,idx);

	// Sort eigenvalues by their absolute values
	{EV, idx} = sort(abs(EV));
	
	U = T(  1:  n, idx(1:n));
	V = T(n+1:2*n, idx(1:n));
	P = V * U~;

	if (isreal(A) && isreal(B) && isreal(Q) && isreal(R)) {
		P = Re(P);
		P = (P' + P)/2;
	} else {
		P = (P# + P)/2;
	}

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

	// Check unit-circle roots:
	F = (R + B#*P*B) \ (B#*P*A);
	pc = eigval(A - B*F);
	{tmp,i} = minimum(abs(1 .- Array(pc)));
	if (tmp < tol2) {
		warning("dric(): There are unit-circle 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("\ndric(): 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 DRic(A, B, Q, R, tol1, tol2, ...)
	Matrix A, B, Q, R;
	Real tol1, tol2;
{
	error(nargchk(3, 5, nargs, "DRic"));
	
	if (nargs == 3) {
		return dric(A, B, Q, R);
	} else if (nargs == 4) {
		return dric(A, B, Q, R, tol1);
	} else {
		return dric(A, B, Q, R, tol1, tol2);
	}
}