% ressort11.m
% fd   19 juin  2006, 02 mars 2025 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% title ('Assimilation variationnelle pour deux  oscillateurs couples \
%%% avec source sinusoidale') ;
nitmax = 20     %                  nombre d iterations du gradient  
roro   = .02    %                  parametre de sous-relaxation
n=100            %                  nombre de points en temps 
tmax= 10    %                  temps d assimilation des donnees
%
%                               parametres initiaux 
  kkini1 = .3  ; kkini2 = 3.2 ;  kka1 = kkini1 ; kka2 = kkini2 ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% jeux de parametres qui tournent 
% nitmax = 70 ;   roro   = .0001 ; n=400  ; tmax= 40 ;  kkini1 = .8  ; kkini2 = 2.3 ; 
% nitmax = 30 ;   roro   = .02   ; n=600  ; tmax= 10 ;  kkini1 = .3  ; kkini2 = 3.2 ; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       donnes de reference      %%%%%%%%%%%%
mm1 = 1 ; mm2 = 1 ; mm3 = 1 ;  kk1 = 1 ; kk2 = 2 ; 
mm=[ mm1 0 0 ; 0  mm2 0 ; 0 0  mm3 ]  ; 
z3 = [  0  0 0 ; 0  0  0 ; 0 0  0 ] ;  cc= z3 ; 
ggkk1 = [ 1 -1 0 ; -1 1 0 ; 0 0 0 ] ; 
ggkk2 = [ 0 0 0 ;  0 1 -1 ; 0 -1 1 ] ; 
kk    = kk1 * ggkk1 +  kk2 * ggkk2 ; 
x0 = [ 1 ; -1 ;  0 ] ;
y0 = [ 0 ; 0 ; 1 ]  ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       solution analytique     %%%%%%%%%%%%
z3 = [  0  0 0 ; 0  0  0 ; 0 0  0 ] ; 
I3 = [  1  0 0 ; 0  1  0 ; 0 0  1 ] ; 
I6 = [  I3  z3 ; z3   I3 ] ; 
aa = [ z3 , I3 ; -mm\kk , z3  ] ;  
u0 = [ x0 ; y0 ] ; 
charge =  [ 1 ; 0 ; -1 ] ; 
phi   = [ 0 ; 0 ; 0 ; mm\charge ] ; 
omeg  = 4*pi ;  
psi2  = -( omeg*I6 + (1/omeg)*aa*aa ) \ phi ;  
psi1  = (1/omeg)*aa*psi2 ; 
uf    = sin(omeg*tmax)*psi1  +  cos(omeg*tmax)*psi2  + expm(tmax*aa)*(u0-psi2) ; 
dtuf  = omeg*cos(omeg*tmax)*psi1  -omeg*sin(omeg*tmax)*psi2  + ...
        aa*expm(tmax*aa)*(u0-psi2) ; 
resi =  dtuf - aa*uf - sin(omeg*tmax)*phi ; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       solution numerique     %%%%%%%%%%%%
n=100

dt=tmax/n ; 
t=zeros(1,n+1);
x=zeros(3,n+1);
y=zeros(3,n+1); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       solution numerique     %%%%%%%%%%%%
for i=1:n;
t(1,i+1:i+1)=t(1,i:i)+dt;
td(1,i:i) = (i-.5)*dt ; 
end ; 
%
ff = sin(omeg*t(1,1:1)) * charge ; 
xx = x0 ;
yy = y0 ;
x(:,1:1) = xx(:,1) ; 
y(:,1:1) = yy(:,1) ; 
ga = mm \ ( ff - cc*yy - kk*xx )  ;
ss = mm + .5*dt*cc + .25*dt*dt*kk ; 
%
for i=1:n;
xt = xx + dt*yy + .25*dt*dt*ga ; 
yt = yy + .5*dt * ga ; 
ff = sin (omeg*t(1,i+1:i+1)) * charge ; 
ga = ss \ ( - kk*xt - cc*yt + ff ) ;
xn = xt + .25*dt*dt*ga ; 
yn = yt + .5*dt*ga ; 
%
x(:,i+1:i+1) = xn(:,1) ; 
y(:,i+1:i+1) = yn(:,1) ; 
xx(:,1) = x(:,i+1:i+1) ; 
yy(:,1) = y(:,i+1:i+1) ; 
% ga1 = mm \ ( ff - cc * yy - kk*xx )  
end ;
% delta = uf - [ x(:,n+1:n+1) ; y(:,n+1:n+1) ]
%
 
plot(t(1,:) , x(1:1,:), '*k;x1 position;' , t(1,:) , x(2:2,:), '*b;x2 position;'  , ...
     t(1,:) , x(3:3,:), '*r;x3 position;' ) ;
axis ( [ -.1   ,  tmax+.1  , -1.3 , 5.1 ]) ;
title ('triple oscillateur harmonique avec source sinusoidale ; schema de Newmark ; solution cible') ; 
print('11a-solution-cible-newmark.jpg', '-djpg', '-S891,630'); 
pause (1.5)  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    probleme inverse      %%%%%%%%%%%%
%                                     initialisation
lll=0 ; 
kkas = zeros (2,nitmax+1) ;   jiter =  zeros (1,nitmax+1) ; 
jiter  (1,lll+1:lll+1)  = lll ; 
kkas (1:1,lll+1:lll+1)  = kka1 ; 
kkas (2:2,lll+1:lll+1)  = kka2 ; 
masse=mm ; amort=cc;  ; 
raideur= kka1 * ggkk1 +  kka2 * ggkk2 ; 

%                               boucle en les parametres 
for lll = 1:nitmax ; lll ; 
xxs=zeros(3,n+1);
yys=zeros(3,n+1); 
%                               evolution directe
xxx = x0 ;
yyy = y0 ; 
xxs(:,1:1)=xxx ; 
yys(:,1:1)=yyy ; 
ssiter = masse + .5*dt*amort + .25*dt*dt*raideur ; 
for i=1:n;
ff = sin(omeg*t(1,i:i)) * charge ; 
ga = masse \ ( ff  - raideur*xxx -  amort*yyy )  ;
xt = xxx + dt*yyy + .25*dt*dt*ga ;
yt = yyy + .5*dt * ga ; 
ff = sin(omeg*t(1,i+1:i+1)) * charge ; 
ga = ssiter \ ( ff - raideur*xt - amort*yt ) ;
xxx  = xt + .25*dt*dt*ga ; 
yyy  = yt + .5*dt*ga ; 
xxs(:,i+1:i+1)  = xxx ; 
yys(:,i+1:i+1)  = yyy ; 
end ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%               erreur


aa = [ z3 , I3 ; -mm\raideur , z3  ] ;  
psi2  = -( omeg*I6 + (1/omeg)*aa*aa ) \ phi ;  
psi1  = (1/omeg)*aa*psi2 ; 
uf    = sin(omeg*tmax)*psi1  +  cos(omeg*tmax)*psi2  + expm(tmax*aa)*(u0-psi2) ; 
delta = uf - [ xxs(:,n+1:n+1) ; yys(:,n+1:n+1) ] ; 

err = xxs - x ;
if (lll==1) ; 
plot(t(1,:) , err(1:1,:), '*k;erreur;' , ...
     t(1,:) , err(2:2,:), '*b;;' , ...
     t(1,:) , err(3:3,:), '*r;;'  ) 
axis ( [ -.1   , tmax+ .1   , -2.5   , 2.5  ]) ;
title ('triple oscillateur harmonique avec source sinusoidale ; erreur initiale') ; 
print('11b-erreur-dynamique-initiale.jpg', '-djpg', '-S891,630') ; 
pause(1.5) ; end ;
if (lll==nitmax) ; 
plot(t(1,:) , err(1:1,:), '*k;erreur;' , ...
     t(1,:) , err(2:2,:), '*b;;' , ...
     t(1,:) , err(3:3,:), '*r;;'  ) 
axis ( [ -.1   , tmax+ .1   , -.0025  , .003  ]) ;
title ('triple oscillateur harmonique avec source sinusoidale ; erreur finale') ; 
print('11e-erreur-dynamique-finale', '-djpg', '-S891,630') ; 
pause(1.5) ; end ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%                 equation adjointe
%
ss = masse + .5*dt*amort + .25*dt*dt*raideur ; 
pp = zeros(3,n); qq = pp ; 
pp(:,n:n) = .5 * dt * err(:,n+1:n+1) ; 
qq(:,n:n) = 0 ; 
ymax = abs(pp(:, n:n)) ; 
ymax = max (ymax(:),abs(qq(:,n:n))) ; 
for j = 1:n-1;
jj    = n-j ; 
ro    = -ss \ (.25*dt*dt*pp(:,jj+1:jj+1) + .5*dt*qq(:,jj+1:jj+1) ) ; 
pt    = pp(:,jj+1:jj+1) + raideur*ro ; 
qt    = qq(:,jj+1:jj+1) + amort*ro ; 
rot   = - masse \ (.25*dt*dt*pt + .5*dt*qt ) ;
pp(:,jj:jj) = pt + dt*err(:,jj+1:jj+1) + raideur*rot ; 
qq(:,jj:jj) = qt + dt*pt               + amort*rot ; 
ymax = max (ymax(:),abs(pp(:,jj:jj))) ; 
ymax = max (ymax(:),abs(qq(:,jj:jj))) ; 
end ;

ymax = max(ymax(:)) ; ymax = 1.05 * ymax ;


if (lll==1) ;
plot (  td(1,:) , pp(1:1,:), '*k;pp1 premier etat adjoint;',   ... 
        td(1,:) , qq(1:1,:), 'ok;qq1 premier etat adjoint;'  ,  ...
        td(1,:) , pp(2:2,:), '*b;pp2 second etat adjoint;' , ...
        td(1,:) , qq(2:2,:), 'ob;qq2 second etat adjoint;' ,  ...
        td(1,:) , pp(3:3,:), '*r;pp3 troisieme etat adjoint;' , ...
        td(1,:) , qq(3:3,:), 'or;qq3 troisieme etat adjoint;' ) ;  
axis ( [ -.1   , tmax+.3  , -10.1 , 9.1 ]) ;  
title ('triple oscillateur harmonique avec source sinusoidale ; premier etat adjoint dynamique') ; 
print('11c-premier-etat-adjoint-dynamique.jpg', '-djpg', '-S891,630') ; 
pause(1.5) ; end ;

if (lll==nitmax) ;
plot (  td(1,:) , pp(1:1,:), '*k;pp1 premier etat adjoint;',   ... 
        td(1,:) , qq(1:1,:), 'ok;qq1 premier etat adjoint;'  ,  ...
        td(1,:) , pp(2:2,:), '*b;pp2 second etat adjoint;' , ...
        td(1,:) , qq(2:2,:), 'ob;qq2 second etat adjoint;' ,  ...
        td(1,:) , pp(3:3,:), '*r;pp3 troisieme etat adjoint;' , ...
        td(1,:) , qq(3:3,:), 'or;qq3 troisieme etat adjoint;' ) ;  
axis ( [ -.1   , tmax+.3  , -.0081  , .0061 ]) ;  
title ('triple oscillateur harmonique avec source sinusoidale ; etat adjoint dynamique final') ; 
print('11d-dernier-etat-adjoint-dynamique.jpg', '-djpg', '-S891,630') ; 
pause(1.5) ; end ;

%                          gradient par rapport a kka1 et kka2
grkka1 = 0 ;  grkka2 = 0 ; 
for  j = 1:n ;
xxx    = xxs(:,j:j) ; 
yyy    = yys(:,j:j) ; 
ff     = sin(omeg*t(1,j:j)) * charge ; 
ga     = masse \ ( ff - raideur*xxx -  amort*yyy )  ;
xt     = xxx + dt*yyy + .25*dt*dt*ga ;
yt     = yyy + .5*dt * ga ; 
ff     = sin(omeg*t(1,j+1:j+1)) * charge ; 
gap    = ss \ ( ff - raideur*xt - amort*yt ) ;
ro     = -ss \ (.25*dt*dt*pp(:,j:j) + .5*dt*qq(:,j:j) ) ; 
pt     = pp(:,j:j) + raideur*ro ; 
qt     = qq(:,j:j) + amort*ro ; 
rot    = - masse \ ( .25*dt*dt*pt + .5*dt*qt ) ;
grkka1 = grkka1 + ro'*ggkk1*(xt+dt*dt*gap/4) + rot'*ggkk1*xxx ; 
grkka2 = grkka2 + ro'*ggkk2*(xt+dt*dt*gap/4) + rot'*ggkk2*xxx ;  
end ; 

%  grkka1 , grkka2 
kka1   = kka1  - roro * grkka1 ; 
kka2   = kka2  - roro * grkka2 ; 
raideur= kka1 * ggkk1 +  kka2 * ggkk2 ; 

jiter (1,lll+1:lll+1,1) = lll ; 
kkas  (1:1,lll+1:lll+1) = kka1 ; 
kkas  (2:2,lll+1:lll+1) = kka2 ; 

end ; 


zzmm = max (kka1 , kkini1) ; zzmm = max (zzmm , kka2 ) ; 
zzmm = max (zzmm , kkini2) ;  zzmm = 1.05 * zzmm ; 
zmin = min( kkini1 , kkini2) ; zmin = min(zmin , kk1) ; zmin = min(zmin , kk2) ;


plot ( jiter(1,:) , kkas (1:1,:) ,   '*k;raideur1;' ,  ...
       [0 , nitmax] , [kk1 , kk1] , 'k;;' ,... 
       jiter(1,:) , kkas (2:2,:) ,  '*r;raideur2;' ,  ...
       [0 , nitmax] , [kk2 , kk2] , 'r;;')
axis ( [ -.5   , nitmax+.5  , -.1  , zzmm  ] ) ;
title ('triple oscillateur harmonique ; convergence des parametres au cours du processus') ; 
print('11f-convergence-des-parametres.jpg', '-djpg', '-S891,630') ; 


