/* -*- MaTX -*-
 *
 *  NAME
 *      obsg() - Minimal order observer by Gopinath's method
 *
 *  SYNOPSIS
 *      {Ah,Bh,Ch,Dh,Jh} = obsg(A,B,C,P)
 *         Matrix Ah,Bh,Ch,Dh,Jh;
 *         Matrix A,B,C;
 *         CoMatrix P;
 *
 *  DESCRIPTION
 *      For the system
 *         .
 *         x = Ax + Bu
 *         y = Cx
 *
 *      obsg(A,B,C,D,P) returns the coefficient matrices of the minimal
 *      order observer 
 *         .
 *         z = Ah*z + Bh*y + Jh*u
 *        xh = Ch*z + Dh*y 
 *
 *      designed by Gopinath's method, where the poles of the observer 
 *      are specified in P.
 *
 *  SEE ALSO
 *      pplace
 */

Func List obsg(A, B, C, P, ...)
	Matrix	A, B, C;
	CoMatrix P;
{
	Integer m, n, p, r;
	Real eps;
	Matrix T, Tinv, L, U;
	Matrix Ab, Bb, Cb;
	Matrix Ab11, Ab12, Ab21, Ab22;
	Matrix Bb1, Bb2;
	Matrix Ah, Bh, Ch, Dh, Jh;
	Matrix Q, R;

	error(nargchk(3, 4, nargs, "obsg"));

	eps = EPS * 100.0;

	m = Cols(B);
	n = Rows(A);
	p = Rows(C);
	r = n - p;	// Degree of the observer

	Tinv = [[kernel(C)']
            [   C      ]];
	
	T = Tinv~;

	Ab = Tinv * A * T;
	Bb = Tinv * B;
	Cb = C * T;

	Ab11 = Ab(  1:r,  1:r);
	Ab12 = Ab(  1:r,r+1:n);
	Ab21 = Ab(r+1:n,  1:r);
	Ab22 = Ab(r+1:n,r+1:n);

	Bb1 = Bb(  1:r,:);
	Bb2 = Bb(r+1:n,:);

	if (nargs == 4) { 
		L = PoleAssign(Ab11', -Ab21', P)';
	} else if (nargs == 3) {
		Q = I(Ab11);
		R = I(Rows(Ab21));
		edit Q, R;
		L = Optimal(Ab11', -Ab21', Q, R)';
	}

	U = [I(r), -L] * Tinv;

	Ah = Ab11 - L*Ab21;
	Bh = Ah*L + Ab12 - L*Ab22;
	Ch = T * [[ I(r) ]
              [Z(p,r)]];
	Dh = T * [[  L ]
              [I(p)]];
	Jh = Bb1 - L*Bb2;

//	print U;
//	print "\nCh*U + Dh*C\n";
//	print Ch*U + Dh*C;

	if (max(abs(Ch*U + Dh*C - I(Rows(Ch)))) > eps) {
		warning("obsg(): Observer may be incorrect.\n");
	}

	return {Ah, Bh, Ch, Dh, Jh};
}

/*
Func List obsg(A, B, C, P)
	Matrix	A, B, C;
	CoMatrix P;
{
	Integer m, n, p, r;
	Matrix T, Tinv, L, U;
	Matrix Ab, Bb, Cb;
	Matrix Ab11, Ab12, Ab21, Ab22;
	Matrix Bb1, Bb2;
	Matrix Ah, Bh, Ch, Dh, Jh;

	m = Cols(B);
	n = Rows(A);
	p = Rows(C);
	r = n - p;

	Tinv = [[   C      ]
            [kernel(C)']];
	
	T = Tinv~;

	Ab = Tinv * A * T;
	Bb = Tinv * B;
	Cb = C * T;

	Ab11 = Ab(  1:p,  1:p);
	Ab12 = Ab(  1:p,p+1:n);
	Ab21 = Ab(p+1:n,  1:p);
	Ab22 = Ab(p+1:n,p+1:n);

	Bb1 = Bb(  1:p,:);
	Bb2 = Bb(p+1:n,:);

	L = PoleAssign(Ab22', -Ab12', P)';

	U = [-L, I(r)] * Tinv;
	print U;

	Ah = Ab22 - L*Ab12;

	Bh = Ah*L + Ab21 - L*Ab11;

	Ch = T * [[Z(p,r)]
                [ I(r) ]];

    Dh = T * [[I(p)]
                [ L  ]];

	Jh = Bb2 - L*Bb1;

	return {Ah, Bh, Ch, Dh, Jh};
}
*/