% simul2: Estimation of the impulse response of a LIT system
% 1- Generate an impulse response h(t), u(t)  and y(t)
% 2- Using u(t) and y(t) estimate h: he using different methods
% 3- Comapre and study sensitivity of the solution with respect to the noise: 
%    ||h-he|| as a function of veps
% 4- Comapre and study sensitivity of the solution with respect to the nature 
%    of the input (Identifiability).
% 0- Quit
%-----------------------------------------------------------------------------
% Author: Ali Mohammad-Djafari
% Date: Sept. 22, 2006

close all; clear all; clc
figure(1);h=gcf;set(h,'Position',[610,600,400,300]);
figure(2);h=gcf;set(h,'Position',[1020,600,400,300]);
figure(3);h=gcf;set(h,'Position',[1020,200,400,300]);
figure(4);h=gcf;set(h,'Position',[610,200,400,300]);
figure(5);h=gcf;set(h,'Position',[200,200,400,300]);

while 1
clc, help simul2
nm=input(' Your choice?');
if(nm<=0), break, end

if(nm==1)
% Generate h(t)
disp(' Generate h(t)')
Nh=32;
t=[0:Nh-1]';
k1=menu('Generate h(t)','h1(t)=exp(-.1*t)','h2(t)=exp(-.1*t)*sin(t)');
switch k1
case 1
 h=exp(-t/10);
 h=h/sum(h);
case 2
 h=exp(-t/10).*sin(t);
 h=h/sum(h);
end
figure(1);plot(t,h),title('h(t)');

% Generate u(t)
disp(' Generate u(t)')
N=128;
t=[0:N-1]';
k2=menu('Generate u(t)','u1(t)=step function','u2=white Gaussian','u3=binary Markov chain','u4=3 sinusoids');
switch k2
case 1
 u=ones(N,1);
 U(:,1)=u;
case 2
 u=randn(N,1);
case 3
 s=rand(N,1);
 u=zeros(N,1);
 u(1)=-1;
 for n=2:N
  if(s(n)>.95),u(n)=-u(n-1);else;u(n)=u(n-1);end
 end
case 4
 u=sin(.1*t)+sin(.2*t)+sin(.4*t);
end
figure(2);plot(t,u),title('u(t)');

% Compute y(t)
disp(' Compute y(t)')
y=conv(h,u);y=y(1:N);
figure(3);plot(t,y);,title('y(t)');

% Add noise
disp(' Add noise')
veps=.01;veps=getnew(veps,' Noise variance');
y=y+veps*randn(size(y));
figure(3);plot(t,u,'r');,title('u(t) and y(t)'); 
hold on; plot(t,y,'b'); hold off 

save simul2.mat Nh h N u veps y
end

if(nm==2)
load simul2

% Creat the information matrix 
disp(' Creat the information matrix')
% it is assumed that the input is causal
Phi=toeplitz(u,[u(1) zeros(1,Nh-1)]);
R=Phi'*Phi;
c=Phi'*y;

% Compute he
disp(' Compute he')
while 1
k3=menu('Identification method','Least-Squares','MNLS','Quad. Regularization','Bayesian:  Gaussian case','Quit');
if(k3==5),break,end
switch k3
case 1
 he=inv(R)*c;
 %he=R\c;
 lambda=0;
case 2
 lambda=veps;lambda=getnew(lambda,' Regul. parameter');
 I=eye(size(R));
 he=inv(R+lambda*I)*c;
case 3
 lambda=veps;lambda=getnew(lambda,' Regul. parameter');
 D=toeplitz([1;-2;1;zeros(Nh-3,1)],[1,-1,zeros(1,Nh-2)]);
 I=D'*D;
 he=inv(R+lambda*I)*c;
case 4
 lambda=veps;lambda=getnew(lambda,' Regul. parameter');
 D=toeplitz([1;-2;1;zeros(Nh-3,1)],[1,-1,zeros(1,Nh-2)]);
 I=D'*D;
 C=inv(R+lambda*I);
 he=C*c;
 va=veps*diag(C);
end

% Compare h and he
disp(' Compare h and he')
figure(4);plot([h,he]),title('h(t) and he(t)');
dh=h-he; e=dh'*dh;
disp(['  veps=',num2str(veps),' lambda=',num2str(lambda),'  error=',num2str(e)])
if(k3==4)
figure(4);hold on;errorbar(h,va),hold off
end
end
end

if(nm==3)
% Sensitivity with respect to veps
disp(' Sensitivity with respect to veps')
disp(['  lambda=',num2str(lambda),' method=',num2str(k3)])
y0=conv(h,u);y0=y0(1:N);
R=Phi'*Phi;
veps=[1e-6,1e-5,1e-4:1e-3,1e-2,2e-2,5e-2,1e-1,2e-1]';
K=length(veps);
e=zeros(K,1);
for k=1:K
 y=y0+veps(k)*randn(size(y0));
 c=Phi'*y;
 IR=inv(R+lambda*I);
 he=IR*c;
 figure(4);plot([h,he]),title('h(t) and he(t)');pause(1)
 dh=h-he; 
 e(k)=dh'*dh;
 disp(['  veps=',num2str(veps(k)),'  error=',num2str(e(k))])
end
figure(5),loglog(veps,e),xlabel('veps'),ylabel('||h-he||'),title('Noise sensitivity');
end

if(nm==4)
% Identifiability
disp(' Identifiability')
N=128;
t=[0:N-1];
Nu=4;
U=zeros(N,Nu);
for k=1:Nu
switch k
case 1
 u=ones(N,1);
case 2
 u=randn(N,1);
case 3
 s=rand(N,1);
 u=zeros(N,1);
 u(1)=-1;
 for n=2:N
  if(s(n)>.95),u(n)=-u(n-1);else;u(n)=u(n-1);end
 end
case 4
 u=sin(.1*t)+sin(.2*t)+sin(.4*t);
end
U(:,k)=u;
figure(2);plot(t,u),title('u(t)');pause(1)
end

disp(['  lambda=',num2str(lambda),' method=',num2str(k3)])
for k=1:Nu
 disp([' Input number=',num2str(k)])
 u=U(:,k);
 figure(2);plot(t,u),title('u(t)');pause(1)
 y0=conv(h,u);y0=y0(1:N);
 Phi=toeplitz(u,[u(1) zeros(1,Nh-1)]);
 R=Phi'*Phi;
 veps=[1e-6,1e-5,1e-4:1e-3,1e-2,2e-2,5e-2,1e-1,2e-1]';
 K=length(veps);
 e=zeros(K,1);
 for k=1:K
  y=y0+veps(k)*randn(size(y0));
  figure(3);plot([u,y]),title('u(t) and y(t)');pause(1)
  c=Phi'*y;
  IR=inv(R+lambda*I);
  he=IR*c;
  figure(4);plot([h,he]),title('h(t) and he(t)');pause(1)
  dh=h-he; 
  e(k)=dh'*dh;
  disp(['  veps=',num2str(veps(k)),'  error=',num2str(e(k))])
 end
 figure(5),loglog(veps,e),xlabel('veps'),ylabel('||h-he||'),title('Noise sensitivity');
 hold on
end
hold off
end

end