%function mse = fp_mse(y,u,p);

% find the best theta for the fixed pole problem and compute
% the mse.
%          y = matrix with the true systems
%          P = pole location
%          N = number of blocks 
%          S = number of samples
%          T = number of true systems

%load true_sys
N = size(P); N=N(1,1)/2;  
T = size(y); S=T(1,1);  T=T(1,2);

for n = 1:N,              % find c and b for the complex poles
 c(n) = - real(P(n*2))^2 - imag(P(n*2))^2;    
 b(n) = - 2*real(P(n*2))/(c(n) - 1);
end

n = 1;
den1 = [1 b(n)*(c(n)-1) -c(n)];
L(:,1)=dimpulse([0 1 -b(n)],den1,400); 
 
num1 = [0 0 1];
L(:,2)=dimpulse(num1,den1,400); 
   
num1 = 1;
if N > 1,
 for n=2:N,
   num2 = conv(num1,[-c(n-1) b(n-1)*(c(n-1)-1) 1]);
   den2 = conv(den1,[1 b(n)*(c(n)-1) -c(n)]);
   L(:,n*2-1)=dimpulse(conv([0 1 -b(n)],num2),den2,400); 

   L(:,n*2)=dimpulse(conv([0 0 1],num2),den2,400); 
   num1 = num2;
   den1 = den2;
 end
end

% Create a orthonormal basis
%L(:,1) = L(:,1)/norm(L(:,1));

%ip1 = ip(L(:,2),L(:,1))*L(:,1);
%L(:,2) = L(:,2)-ip1;   
%L(:,2) = L(:,2)/norm(L(:,2));

%ip1 = ip(L(:,3),L(:,1))*L(:,1);
%ip2 = ip(L(:,3),L(:,2))*L(:,2);
%L(:,3) = L(:,3)-ip1-ip2;   
%L(:,3) = L(:,3)/norm(L(:,3));
%
%ip1 = ip(L(:,4),L(:,1))*L(:,1);
%ip2 = ip(L(:,4),L(:,2))*L(:,2);
%ip3 = ip(L(:,4),L(:,3))*L(:,3);
%L(:,4) = L(:,4)-ip1-ip2-ip3;   
%L(:,4) = L(:,4)/norm(L(:,4));
%
%
%ip1 = ip(L(:,5),L(:,1))*L(:,1);
%ip2 = ip(L(:,5),L(:,2))*L(:,2);
%ip3 = ip(L(:,5),L(:,3))*L(:,3);
%ip4 = ip(L(:,5),L(:,4))*L(:,4);
%L(:,5) = L(:,5)-ip1-ip2-ip3-ip4;   
%L(:,5) = L(:,5)/norm(L(:,5));
%
%ip1 = ip(L(:,6),L(:,1))*L(:,1);
%ip2 = ip(L(:,6),L(:,2))*L(:,2);
%ip3 = ip(L(:,6),L(:,3))*L(:,3);
%ip4 = ip(L(:,6),L(:,4))*L(:,4);
%ip5 = ip(L(:,6),L(:,5))*L(:,5);
%L(:,6) = L(:,6)-ip1-ip2-ip3-ip4-ip5;   
%L(:,6) = L(:,6)/norm(L(:,6));

%ip1 = ip(L(:,7),L(:,1))*L(:,1);
%ip2 = ip(L(:,7),L(:,2))*L(:,2);
%ip3 = ip(L(:,7),L(:,3))*L(:,3);
%ip4 = ip(L(:,7),L(:,4))*L(:,4);
%ip5 = ip(L(:,7),L(:,5))*L(:,5);
%ip6 = ip(L(:,7),L(:,6))*L(:,6);
%L(:,7) = L(:,7)-ip1-ip2-ip3-ip4-ip5-ip6;   
%L(:,7) = L(:,7)/norm(L(:,7));

%ip1 = ip(L(:,8),L(:,1))*L(:,1);
%ip2 = ip(L(:,8),L(:,2))*L(:,2);
%ip3 = ip(L(:,8),L(:,3))*L(:,3);
%ip4 = ip(L(:,8),L(:,4))*L(:,4);
%ip5 = ip(L(:,8),L(:,5))*L(:,5);
%ip6 = ip(L(:,8),L(:,6))*L(:,6);
%ip7 = ip(L(:,8),L(:,7))*L(:,7);
%L(:,8) = L(:,8)-ip1-ip2-ip3-ip4-ip5-ip6-ip7;
%L(:,8) = L(:,8)/norm(L(:,8));

for t = 1:T,
 theta = inv(L'*L)*L'*y(:,t)
 y_hat = theta'*L';
 mse(t) = norm(y(:,t) - y_hat')^2/S;
end
