echo on
% On cree une matrice
A=[10 7 8 7;7 5 6 5;8 6 10 9;7 5 9 10]
pause

% On calcule son inverse
AI=inv(A)
% Tous va bien, la matrice est inversible, 
% son inverse est parfait, pas d'erreur d'arrondi !
% on peut verifier AI*A=I
AI*A
pause

% On cree un vecteur x
x=[1;1;1;1]
pause

% On calcule y=A*x
y=A*x
pause

% On calcule la solution x0=inv(A)*y
x0=AI*y
% C'est parfait. Mais ...
pause

% On modifie legerement les mesures y
yb=[31.9, 22.9, 33.1, 29.9]';
% On calcule la solution x1=inv(A)*yb
x1=AI*yb
% Surprise
pause

% Calculons le nombre de condition de la matrice A
cond(A);
pause
% Effectuons une decomposition en valeures singulieres
[U,S,V]=svd(A)
pause
% Verifions la relation A=U*S*V'
U*S*V'
pause

% calculons la solution IG en guardant tous les v.s.
s=diag(S),
x=V*((U'*y)./s),
x0=V*((U'*yb)./s)
pause

% On elimine une valeure singuliere (la plus petite) et 
% on calcule la solution regularisee
p=3;
x1=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p))
pause

% On elimine deux valeures singulieres (les deux plus petites)
p=2;
x2=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p))
pause

% On elimine trois valeures singulieres  (les trois plus petites)
p=1;
x3=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p))
pause

% On cree differentes realisations du bruit et pour chacune 
% on calcule la solution exacte et les solutions regularisees
n=8;
xbt=zeros(4,n);
ybt=zeros(4,n);
for i=1:n
    b=.1*randn(4,1);
    yb=y+b; ybt(:,i)=yb;
end
save dat/test_cond_ybt.mat ybt

for i=1:n
    yb=ybt(:,i);
    xbt1(:,i)=AI*yb;			% Inversion directe
    xbt2(:,i)=inv(A'*A)*A'*yb;	% pseudo inverse
    p=3;xbt3(:,i)=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p));	% 1 vs enlevee
    p=2;xbt4(:,i)=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p));	% 2 vs enlevees
    p=1;xbt5(:,i)=V(:,1:p)*((U(:,1:p)'*yb)./s(1:p));	% 3 vs enlevees
	 % Regularisation ||y-Ax||^2 - lambda*||Dx||^2
    D=eye(4);					% D = Identite
    lambda=.1;					% Lambda = 0.1
    xbt6(:,i)=inv(A'*A+lambda*D)*A'*yb;
    lambda=1;					% Lambda = 1
    xbt7(:,i)=inv(A'*A+lambda*D)*A'*yb;
    lambda=10;					% Lambda = 10
    xbt8(:,i)=inv(A'*A+lambda*D)*A'*yb;

    D=Toeplitz([1,-.5,0,0]);	% D = Toeplitz([1,-0.5,0,0])
    lambda=.1;						% Lambda = 0.1
    xbt9(:,i)=inv(A'*A+lambda*D)*A'*yb;
    lambda=1;						% Lambda = 1
    xbt10(:,i)=inv(A'*A+lambda*D)*A'*yb;
    lambda=10;						% Lambda = 10
    xbt11(:,i)=inv(A'*A+lambda*D)*A'*yb;
end

figure(1);title('Inversion directe')
subplot(221);plot(x, '.');axis([0,5,-15,15]);title('x')
subplot(222);plot(ybt, '.');axis([0,5,20,35]);title('yb')
subplot(223);plot(xbt1, '.');axis([0,5,-15,15]);title('Inv. directe')
subplot(224);plot(xbt2, '.');axis([0,5,-15,15]);title('Moindres Carres ou Inv. Generalisee')
print -depsc Eps/test_cond1.eps
pause

figure(2);title('Inversion par TSVD')
subplot(221);plot(x, '.');axis([0,5,0,2]);title('x')
subplot(222);plot(xbt3, '.');axis([0,5,0,2]);title('TSVD1')
subplot(223);plot(xbt4, '.');axis([0,5,0,2]);title('TSVD2')
subplot(224);plot(xbt5, '.');axis([0,5,0,2]);title('TSVD3')
print -depsc Eps/test_cond2.eps
pause

figure(3);title('Inversion par regularization d''ordre zero')
subplot(221);plot(x, '.');axis([0,5,0,2]);title('x')
subplot(222);plot(xbt6, '.');axis([0,5,0,2]);title('R0.1')
subplot(223);plot(xbt7, '.');axis([0,5,0,2]);title('R01')
subplot(224);plot(xbt8, '.');axis([0,5,0,2]);title('R10')
print -depsc Eps/test_cond3.eps
pause

figure(4);title('Inversion par regularization d''ordre un')
subplot(221);plot(x, '.');axis([0,5,0,2]);title('x')
subplot(222);plot(xbt9, '.');axis([0,5,0,2]);title('R0.1')
subplot(223);plot(xbt10, '.');axis([0,5,0,2]);title('R01')
subplot(224);plot(xbt11, '.');axis([0,5,0,2]);title('R10')
print -depsc Eps/test_cond4.eps
pause

load dat/test_cond_ybt.mat

% Methodes iteratives
 x0=zeros(4,1); %initialisation
 lambda=.05; niter=20;
 echo off

 disp('MC')
 x=x0;
 for iter=1:niter
  x=x+lambda*A'*(y-A*x);
 end
 x, pause

 disp('MC avec contrainte de positivite')
 x=x0;
 for iter=1:niter
  x=x+lambda*A'*(y-A*x);
  iz=find(x < 0);if(~isempty(iz)),x(iz)=zeros(length(iz),1);end
 end
 x, pause

 disp('Kaczmarz')
 x=x0;
 [ly,lx]=size(A);
 for iter=1:niter
  for i=1:ly
   hi=A(i,:);
   d=hi*hi';
   yhi=hi*x;
   xh=((y(i)-yhi)/d)*hi;
   x=x+lambda*xh';
   iz=find(x < 0);if(~isempty(iz)),x(iz)=zeros(length(iz),1);end
  end
 end
 x, pause

 disp('Regularization quadratique d''ordre 0')
 x=x0;lambda=.01;alpha=10;
 for iter=1:niter
  x=x+lambda*(A'*(y-A*x)-alpha*x)
 end
 x, pause

disp('Regularization quadratique d''ordre 1')
 x=x0;lambda=.01;alpha=10;
 for iter=1:niter
  x=x+lambda*(A'*(y-A*x)-alpha*conv2(x,[1 -2 1],'same'))
 end
 x, pause

% Etude de la sensibilite en fonction du critere

% Tracer du critere
x=[1,1,1,1]';
x1=[-2:.1:3];
x3=[-2:.1:3];
for i=1:length(x1)
 x(3)=x3(i);
 for j=1:length(x1)
  x(1)=x1(j);
  yh=A*x; dy=y-yh; J(i,j)=sum(dy.*dy);
 end
end
figure(1),clf,mesh(x1,x3,J),xlabel('x1'),ylabel('x3')
print -depsc Eps/test_cond5.eps
pause

x=[1,1,1,1]';
x2=[-2:.1:3];
x4=[-2:.1:3];
for i=1:length(x1)
 x(4)=x4(i);
 for j=1:length(x1)
  x(2)=x2(j);
  yh=A*x; dy=y-yh; J(i,j)=sum(dy.*dy);
 end
end
figure(2),clf,mesh(x2,x4,J),xlabel('x2'),ylabel('x4')
print -depsc Eps/test_cond6.eps
pause

% Trace des hyperplanes

x1=[0:.1:2];
x2=zeros(length(x1),4);
x3=zeros(length(x1),4);
x4=zeros(length(x1),4);
for j=1:length(x1)
 x=[x1(j),1,1,1]';
 yh=A*x; 
 x(2)=0;
 x2(j,1)=(1/A(1,2))*(y(1)-A(1,:)*x);
 x2(j,2)=(1/A(2,2))*(y(2)-A(2,:)*x);
 x2(j,3)=(1/A(3,2))*(y(3)-A(3,:)*x);
 x2(j,4)=(1/A(4,2))*(y(4)-A(4,:)*x);

 x=[x1(j),1,1,1]';x(3)=0;
 x3(j,1)=(1/A(1,3))*(y(1)-A(1,:)*x);
 x3(j,2)=(1/A(2,3))*(y(2)-A(2,:)*x);
 x3(j,3)=(1/A(3,3))*(y(3)-A(3,:)*x);
 x3(j,4)=(1/A(4,3))*(y(4)-A(4,:)*x);

 x=[x1(j),1,1,1]';x(4)=0;
 x4(j,1)=(1/A(1,4))*(y(1)-A(1,:)*x);
 x4(j,2)=(1/A(2,4))*(y(2)-A(2,:)*x);
 x4(j,3)=(1/A(3,4))*(y(3)-A(3,:)*x);
 x4(j,4)=(1/A(4,4))*(y(4)-A(4,:)*x);
end
figure(3),clf
subplot(221),plot(x1,x2),xlabel('x_1'),ylabel('x_2')
subplot(222),plot(x1,x3),xlabel('x_1'),ylabel('x_3')
subplot(223),plot(x1,x4),xlabel('x_1'),ylabel('x_4')
subplot(224),plot(x2,x4),xlabel('x_1'),ylabel('x_4')

print -depsc Eps/test_cond7.eps