/* -*- MaTX -*-
 *
 *  NAME
 *      feedback() - Connect two systems using negative feedback
 *
 *  SYNOPSIS
 *      {A,B,C,D} = feedback(A1,B1,C1,D1,A2,B2,C2,D2)
 *          Matrix A,B,C,D;
 *          Matrix A1,B1,C1,D1,A2,B2,C2,D2;
 *
 *  DESCRIPTION
 *       feedback() generates an system consisting of the negative 
 *       feedback connection of the two systems 1 and 2.
 *
 *       Consider two sub-systems:
 *          .
 *          x1 = A1*x1 + B1*u1          u  +            y
 *          y1 = C1*x1 + D1*u1          ---->o-->s1---+--->
 *          .                              - ^        |
 *          x2 = A2*x2 + B2*u2               |        |
 *          y2 = C2*x2 + D2*u2               +---s2<--+
 *
 *       and the connection is  u1 = u - y2, then the new system is
 *
 *         x = A*x + B*u
 *         y = C*x + B*u
 *
 *       where the output y is the output of system 1.
 *
 *  SEE ALSO
 *      series, parallel, and augment
 */

Func List feedback(A1,B1,C1,D1,A2,B2,C2,D2)
	Matrix A1,B1,C1,D1,A2,B2,C2,D2;
{
	Integer m1,m2,p1,p2;
	Matrix A,B,C,D,T;
	String msg;

	if (length((msg = abcdchk(A1, B1, C1, D1))) > 0) {
		error("feedback(): " + msg);
	}
	if (length((msg = abcdchk(A2, B2, C2, D2))) > 0) {
		error("feedback(): " + msg);
	}
	
	{p1,m1} = size(D1);
	{p2,m2} = size(D2);

	T = I(p1) + D1*D2;
	A = [[A1-B1*D2/T*C1  B1*(D2/T*D1-I(m1))*C2]
		 [   B2/T*C1        A2-B2/T*D1*2      ]];
	B = [[B1*(I(p2)-D2/T*D1)]
		 [     B2/T*D1      ]];
	C = [T\C1, -T\D1*C2];
	D = T\D1;
	return {A,B,C,D};
}