Falb_Wolovich_Roppenecker(A,B,C,Te)

function [L, M] = Falb_Wolovich_Roppenecker(A,B,C,Te)
%
% Synthse d'une commande multivariable avec dcouplage, par la mthode de Falb-Wolovich
% ou la synthse modale complte (Roppenecker) : ne s'applique que si p = q.
%
%  [L, M] = Falb_Wolovich_Roppenecker(A,B,C,Te)  calcule selon l'une de deux mthodes,
%
%      * mthode de Falb-Wolovich (choix F),
%      * mthode de la synthse modale complte, ou mthode de Roppenecker (choix R),
%
%  la matrice de retour d'tat L qui produit un systme boucl sous forme de
%  sous-sytmes monovariables en parallle. Les valeurs propres du systme en
%  boucle ouverte qui peuvent tre dplaces vers de nouvelles valeurs imposes au
%  systme boucl dpendent de la diffrence d'ordre du systme initial, et sont
%  mentionnes  l'utilisateur pour qu'il choisisse ces nouvelles valeurs.
%
%  Une valeur de Te > 0 en entre indique que ces valeurs doivent tre converties en leur
%  quivalent discret  la priode Te, par exp(valeur*Te).
%
%  La matrice M renvoye en sortie est la matrice de compensation de gain qui
%  assure un gain statique unit au systme boucl et qui est aussi calcule par
%  cet algorithme.
%
%  Utilise : saisievalpro.
%
%  Auteur : E. Ostertag  27 juin 2005  
%

n = size(A,1); p = size(B,2); q = size(C,1); D = zeros(q,p);
if p ~= q,
	disp('*** La mthode du dcouplage ne s''applique que si p=q ***'); beep;
	L=0; M=0;
	return
end
%
% 1.- Dtermination de la diffrence d'ordre delta(i) des diverses sorties yi
%
for i=1:q,
	di = 0;
	Mtest = C(i,:)*A^di*B;
	while C(i,:)*A^di*B == 0,
		di = di+1;
	end
	delta(i)=di+1;
end
%
% 2.- Diffrence d'ordre globale "sumdelta" du systme multivariable
%
Delta = 0;
for i=1:q,
	Delta = Delta+delta(i);
end
if Delta == n,
	disp('Delta = n, donc toutes les valeurs propres peuvent tre dplaces');
else
	disp(['Delta < n : seules ' num2str(Delta) ' valeurs propres peuvent tre dplaces']);
end
%
% 3.- Dcouplage possible seulement si la matrice D* calcule ci-dessous est inversible
%
for i=1:q,
	Dstar(i,:) = C(i,:)*A^(delta(i)-1)*B;
end
if det(Dstar) ~= 0,
%
% 4.- Dtermination par placement de ples des dnominateurs des q sous-
%     systmes monovariables dcoupls, d'ordres respectifs delta(i) :
%
	disp('Mthode choisie :');
  disp('                 F = mthode de Falb-Wolovich (par dfaut)');
  disp('                 R = mthode de Roppenecker (synthse modale complte)');
  method = input('Votre choix --> ','s');
	if isempty(method)
		method = 'F';
	else
		method = upper(method(1));
	end
	sys_BO = ss(A, B, C, D, Te);
	G_BO = tf(sys_BO);
	kP = 1;
	for i=1:q,
		lambdades = saisievalpro(delta(i),Te);
		linep{i} = real(poly(lambdades));
		if Te > 0,
			knum(i) = polyval(linep{i},1);
		else
			knum(i) = polyval(linep{i},0);
		end
		if method == 'F',
			P(i,:) = C(i,:)*polyvalm(linep{i},A);
		else
			e_vect =[zeros(1,i-1) 1 zeros(q-i,1)]';
			for k=1:delta(i),
				G_BO_lambdades = evalfr(G_BO,lambdades(k));
				P(:,kP) = -inv(G_BO_lambdades)*e_vect;
				V(:,kP) = inv(A-lambdades(k)*eye(n))*B*P(:,kP);
				kP = kP+1;
			end
		end
	end
	if method == 'F',
		Ldecouplage = inv(Dstar)*P;	
	else
		Z_BO = zero(sys_BO);
		nz_BO = length(Z_BO);
		for i=1:nz_BO,
% % % 			Rbkmat = [A-Z_BO(i)*eye(n) B; C D]; null_vect = null(Rbkmat,'r'); nv = size(null_vect,2);
			Rbkmat = [A-Z_BO(i)*eye(n) B; C D]; null_vect = null(Rbkmat); nv = size(null_vect,2);
			Xz = null_vect(1:n,1:nv), Uz = null_vect(n+1:n+p,1:nv),
			V(:,kP:kP+nv-1) = Xz;	P(:,kP:kP+nv-1) = -Uz;
			kP = kP+nv;
			if kP > n,
				break;
			end
		end
		Ldecouplage = P*inv(V);
	end	
else
	disp('D* n''est pas inversible ===> dcouplage impossible'); beep;
	L=0; M=0;
	return
end
L = arrondi_matrice(Ldecouplage); M = inv(Dstar)*diag([knum]);