function [E_rec,F_rec]= diophantinemulti(A,B,N,ny)
%Solves the Diophantine equation, A and B is modelparameters, N is the cost horizon
%Uses the recursively method described by Clarcke et. al. in 
%"Generalized Predictive Control - Part I. The Basic Algorithm"
%Automatica. Vol. 23, No.2, pp 137-148, 1987.
%Rembember A= I(nyxny)+A1*z(-1)+A2*z(-2)+.....+Ana*z(-na)
%where I, A1, A2 ..... Ana are matrixes all of size (ny)x(ny) (ny=number of outputs)

%Initializing E 
E=eye(ny);

%Initializing F
Atilda=[A zeros(ny)]-[zeros(ny) A]; %Atilda=A(1-q^-1)
d=size(Atilda);
v=[eye(ny) zeros(d(1),d(2)-ny)];
Aprime=v-Atilda; %Aprime= 1-Atilda
F=Aprime(:,ny+1:size(Aprime,2));   %F=q(1-Atilda)
m=size(Aprime,2)-size(F,2);
F=[F zeros(ny,m)];  %Size F needs to be equal Atilda= size Aprime

%Saving the E & F for each loop, including the initial values
E_rec=zeros(N*ny); %Initialize the E that is recorded
E_rec(1:ny,1:ny)=E;
F_rec=zeros(N*ny,size(F,2));
F_rec(1:ny,1:size(F,2))=F; %Initilize F that is recorded

%Recursion loop for Diophantine
for j=2:N
   r=F(:,1:ny); 
   R=[E r]; %updates the new R value, which takes the first element in F and add in
   S=F(:,ny+1:end)-r*Atilda(:,ny+1:end);
   r=[R zeros(ny,N*ny-j*ny)]; % Set R which is E(j+1) so R represent E(j+2) in next loop
   E_rec(j*ny-(ny-1):j*ny,:)=r;
   E=R;
   %Set S which is F(j+1) so S represent F(j+2) in next loop
   F=[S zeros(ny,size(Atilda,2)-size(S,2))];
   F_rec(j*ny-(ny-1):j*ny,:)=F; 
end  

%Erase the last column.
%In the calculation the size of F needs to equal the size of Atilda. This adds ny columns.
F_rec=F_rec(:,1:size(F_rec,2)-ny);  

