* ================================================================== * program DPenAssim * Program to do (1+1)dVar with the double pendulum system * Modified double pendulum (system 2) * Ross Bannister * * setup studio12 * f77 Assim.f -o Assim.out -xtypemap=real:64 * History * ------- * 12.09.01 Original code. * 15.11.01 Include output of init conditions (each cycle) with errs, * J, Jb, Jo for each iter. * 18.12.01 Create new section for simple insertion of observations. * 07.06.10 Make minor changes, including format of input and always * use degrees for user data implicit none * Constants * --------- integer maxobs,winsize parameter (maxobs=5000, winsize=5000) * maxobs: maximum # of observations * winsize: maximum # of timesteps in an assim cycle * Integer variables * ----------------- integer Nt,Nout,Nobs,Nwindow,ObTypes(maxobs),Cycles,AssCy,Nsteps integer ObFirst,ObLast,Index(maxobs),remaining,loop,safety,t * Nt: total # time steps * Nout: Frequency of obs output * Nobs: # observations in total * Nwindow: # time steps in typical assimilation cycle * ObTypes: 'degree-of-freedom' index for ob * Cycles: # of cycles for whole run * Nsteps: # time steps in current assimilation cycle * ObFirst: index to first ob in current cycle * ObLast: index to last ob in current cycle * Index: links observations to model states (like pointer) * safety: max number of Var. iterations * t: time loop (used when 'inserting' observations) * Real variables * -------------- real x0(4),g,dt,pi,l1,l2,l3,m1,m2,m3,noisepos,noisevel,t1,t2 real ActualObs(maxobs),ObTimes(maxobs),ObErrs(maxobs),Binv(4,4) real x(winsize,4),xB(4),xANp(4),xLS(4),xBp(4),GradJOb(maxobs) real U1(4,4),U2(4,4),GradJTrans(4),ms,crit,H(maxobs,4),Hess(4,4) real xfore(winsize,4),x0f(4),J,Jb,Jo,HessInv(4,4) * x0: Initial conditions for whole run * g,l1,l2,l3,m1,m2,m3: model parameters * noisepos, noisevel: random noise for angles and angular velocities * ActualObs: the observations * ObTimes: the times of the observations (real time) * ObErrs: the uncertainties in the observtions * x: for model integration * xb: background state of current cycle * xANp: current version of the analysis increment xANp = xAN - xLS * xLS: current linearization state * xBp: background increment (xBp=xB-xLS) * GradJOb: gradient of J wrt model obs * U2: eigenvecs of transformation (rows of trans(U2) are eigenvecs) * U1: eigenvalues of transformation (diagonal elements) * GradJTrans: gradient of J wrt transformed state vector * crit: cut-off for convergence of Var. * H: d(modelob(t))/dx(0) * Hess: Hessian matrix (model space) * HessInv: Inverse of Hessian * Binv: Inverse of background error co-variance matrix * t1,t2: Times bounding the assimilation cycle * xfore: Forecast using originial initial conditions (for info) * x0f: Initial conditions for current cycle from originial initial * conditions (with no assimilation) * J, Jb, Jo: cost function and components * Characters * ---------- character*7 Status character*8 fname * Boolean * ------- logical finish * Workspace arrays * ---------------- real Work1(maxobs),Work2(maxobs),Work3(winsize,4) * Functions * --------- real modsq,mymod * ================================================================== call Init (pi,crit,safety) * Read-in the input data call ReadConfig (Status,x0,g,dt,Nt,Nout,l1,l2,l3,m1,m2,m3,pi, : noisepos,noisevel,ActualObs,ObTypes,ObTimes, : ObErrs,Nobs,Nwindow,maxobs) if (Status.eq.'MakeObs') then * ---------------------------------------------------------------- * || *** SECTION I: MAKE OBSERVATIONS *** || * ---------------------------------------------------------------- * This run is to make-up some observations * ---------------------------------------- print*,'MAKING OBSERVATIONS' print*,'-------------------' call MakeObs (x0,g,dt,pi,Nt,Nout,l1,l2,l3,m1,m2,m3,noisepos, : noisevel) elseif (Status.eq.'Analyse') then * ---------------------------------------------------------------- * || *** SECTION II - ANALYSE OBSERVATIONS *** || * ---------------------------------------------------------------- * This run is to analyse some observations * ---------------------------------------- print*,'ANALYSE OBSERVATIONS' print*,'--------------------' * Open the analysis integration output file open (13,file='Assim.anal') * Open files containing observations used open (14,file='Assim.obs1') open (15,file='Assim.obs2') open (16,file='Assim.obs3') open (17,file='Assim.obs4') * Open file containing cycle boundary times open (18,file='Assim.bnds') * Open file for complete forecast from originial initial conds open (19,file='Assim.fore') * Unit 20 is to output the test integration after each iteration * Open file for the analysis initial conditions (and errors) open (21,file='Assim.init') * Calculate the number of assimilation cycles Cycles=int(real(Nt)/real(Nwindow)) if (Cycles*Nwindow.lt.Nt) Cycles=Cycles+1 * The number of time steps in an assimilation cycle is usually Nsteps=Nwindow * Point to end of observations (initial value) ObLast=0 do AssCy=1,Cycles print*,'-----------------------------------------------------' print*,'Assimilation Cycle No. ',AssCy print*,'-----------------------------------------------------' ObFirst=ObLast+1 * Check to see if there are < Nwindow left in the time domain remaining=Nt-(AssCy-1)*Nwindow if (remaining.lt.Nwindow) Nsteps=remaining * Work out times bounding this assimilation cycle t1=real((AssCy-1)*Nwindow)*dt t2=real((AssCy-1)*Nwindow+Nsteps)*dt * Output the boundary time call BoundTime (t1,18) * Find the last observation pertaining to current cycle * (and set-up the observation indices) call FindLast (ActualObs,ObTimes,ObErrs,ObTypes,ObFirst, : ObLast,Index,Nobs,t1,t2,dt,pi,14,maxobs) print*,'Start and end times : ',t1,t2 print*,'First ob index,time : ',ObFirst,ObTimes(ObFirst) print*,'Last ob index,time : ',ObLast,ObTimes(ObLast) * Store the background for this cycle from either given initial * conditions, or from previous forecast call Background (x0,x,AssCy,xB,Nwindow,winsize) * Set the linearization state (l.s.) to the background call Equal (xLS,xB,4,4) print*,'LS ',xLS(1),xLS(2),xLS(3),xLS(4) if (ObLast.ge.ObFirst) then * There are obs in this window - do data assimilation * --------------------------------------------------- print*,'Analysing ',ObLast-ObFirst+1,' observations' loop=0 10 loop=loop+1 print*,' -----------------------------------' print*,' Loop number ',loop * Find the background as an increment to the l.s. (xBp=xB-xLS) call PlusMinus (xBp,xB,xLS,-1.,4,4) * Do a forward integration, starting from xLS call Advance (x,xLS,Nsteps,1,0.,m1,m2,m3,l1,l2,l3,g,dt,pi, : winsize) * Output the current guess at state call SetUpName (AssCy,loop,fname) open (20,file=fname) call OutputIntegration (x,Nsteps,Nwindow,AssCy,20,dt,pi, : winsize) close (20) if (loop.eq.1) then * If first run, calculate Binv based on this integration if (AssCy.eq.1) call CalcBinv (x,Binv,Nsteps,pi,winsize) * Start/continue forecast (from original initial conds) endif * Calculate the the gradient of J w.r.t. model observations * (this is E^(-1) (Obs - Modelobs) * Also convenient to calculate Jo also (for diagnosis) call GradientJOb (x,ActualObs,ObTypes,ObErrs,Index,GradJOb, : Jo,ObFirst,ObLast,pi,winsize,maxobs) * Calculate Jb call CalcJb (xB,x,Jb,Binv,winsize) * The total cost function J=Jb+Jo print 113,' J,Jb,Jo ',J,Jb,Jo * Compute the forward model matrix, H call ForwardModel (xLS,Nsteps,m1,m2,m3,l1,l2,l3,g,dt,pi, : ObFirst,ObLast,ObTypes,Index,Work1,Work2, : Work3,H,winsize,maxobs) * Compute the Hessian in model space call Hessian (Binv,ObErrs,H,Hess,Obfirst,Oblast,maxobs) * Diagonalize the Hessian call Jacobi (Hess,U1,U2,4,4,0.00001) * Check eigenvectors * call CheckDiag (Hess,U2) * Check signs of eigenvalues call CheckPos (U1,4) * Currently: Diagonal elements of U1 are eigenvalues * Rows of U2 are eigenvectors * Want : Diagonal elements of U1 to be 1/sqrt(e-values) * Columns of U2 to be e-vectors call InvSqrt (U1,4) call Transpose (U2,4) * Check that the Hessian in transformed space = I * call CheckHess (Hess,U1,U2) * Calculate the gradient of J wrt the transformed space * (note U = U2 U1) call GradientJTrans (GradJTrans,GradJOb,ObFirst,ObLast,Binv, : U1,U2,xBp,H,loop,maxobs) * The analysis increment in v-space is -GradJTrans = -dJ/dv * Transform dJ/dv to x-space call Utransform (xANp,GradJTrans,U1,U2,4) * Find new linearization state (ie the analysis) xLS=xLS-xANp call PlusMinus (xLS,xLS,xANp,-1.,4,4) * Put angles into range (-pi to pi) xLS(1)=mymod(xLS(1),pi) xLS(2)=mymod(xLS(2),pi) * Calculate the modsquared of xANp ms=modsq(xANp,4) print*,' Outer loop ',loop,', increment len. ',ms if ((ms.gt.crit).and.(loop.lt.safety)) goto 10 * ------------------------------------------------------------ else print*,'NO OBESERVATIONS' endif * Output analysed initial conditions (with errors) write (21,111) 'Anal. init. conds. Time = ', : real((AssCy-1)*Nwindow)*dt,' with error' write (21,112) 180.*xLS(1)/pi, 180.*xLS(2)/pi, : 180.*xLS(3)/pi,180.*xLS(4)/pi * Calculate and output the analysis errors (from the Hessian) * First calculate the forward model (required to compute Hess.) call ForwardModel (xLS,Nsteps,m1,m2,m3,l1,l2,l3,g,dt,pi, : ObFirst,ObLast,ObTypes,Index,Work1,Work2, : Work3,H,winsize,maxobs) call Hessian (Binv,ObErrs,H,Hess,Obfirst,Oblast,maxobs) * Invert the Hessian (use U1 and U2 as workspace) call Inverse (Hess,Hessinv,U1,U2,4) write (21,112) 180.*sqrt(Hessinv(1,1))/pi, : 180.*sqrt(Hessinv(2,2))/pi, : 180.*sqrt(Hessinv(3,3))/pi, : 180.*sqrt(Hessinv(4,4))/pi * Make a forcast based on xLS for next background remaining=Nt-(AssCy-1)*Nwindow if (remaining.lt.Nwindow) Nsteps=remaining call Advance (x,xLS,Nsteps,1,0.,m1,m2,m3,l1,l2,l3,g,dt,pi, : winsize) * Output analysis call OutputIntegration (x,Nsteps,Nwindow,AssCy,13,dt,pi, : winsize) * Calculate B and Binv based on this integration call CalcBinv (x,Binv,Nsteps,pi,winsize) * Continue forecast with no assimilation call Background (x0,xfore,AssCy,x0f,Nwindow,winsize) call Advance (xfore,x0f,Nsteps,1,0.,m1,m2,m3,l1,l2,l3,g,dt,pi, : winsize) call OutputIntegration (xfore,Nsteps,Nwindow,AssCy,19,dt,pi, : winsize) enddo * Close the output files close (13) close (14) close (15) close (16) close (17) close (18) close (19) close (21) else * ---------------------------------------------------------------- * || *** SECTION III - INSERT OBSERVATIONS *** || * ---------------------------------------------------------------- * This run is to insert observations * ---------------------------------- print*,'INSERT OBSERVATIONS' print*,'-------------------' * Open the integration output file for the 'insertion' run open (13,file='Assim.ins') * Open files containing observations used open (14,file='Assim.obs1') open (15,file='Assim.obs2') open (16,file='Assim.obs3') open (17,file='Assim.obs4') * Calculate the number of assimilation cycles Cycles=int(real(Nt)/real(Nwindow)) if (Cycles*Nwindow.lt.Nt) Cycles=Cycles+1 * The number of time steps in an assimilation cycle is usually Nsteps=Nwindow * Point to end of observations (initial value) ObLast=0 do AssCy=1,Cycles print*,'-----------------------------------------------------' print*,'Assimilation Cycle No. ',AssCy print*,'-----------------------------------------------------' ObFirst=ObLast+1 * Check to see if there are < Nwindow left in the time domain remaining=Nt-(AssCy-1)*Nwindow if (remaining.lt.Nwindow) Nsteps=remaining * Work out times bounding this assimilation cycle t1=real((AssCy-1)*Nwindow)*dt t2=real((AssCy-1)*Nwindow+Nsteps)*dt * Find the last observation pertaining to current cycle * (and set-up the observation indices) call FindLast (ActualObs,ObTimes,ObErrs,ObTypes,ObFirst, : ObLast,Index,Nobs,t1,t2,dt,pi,14,maxobs) print*,'Start and end times : ',t1,t2 print*,'First ob index,time : ',ObFirst,ObTimes(ObFirst) print*,'Last ob index,time : ',ObLast,ObTimes(ObLast) print*,'Inserting ',ObLast-ObFirst+1,' observations' * Set up the initial conditions in x array call InitialCondsInsert (x0,x,AssCy,Nsteps,winsize) * Integrate over time and insert observations as and when * they appear. loop=ObFirst do t=1,Nsteps * If there are observations at this time, insert them. finish=.false. 20 if (loop.le.ObLast) then if (Index(loop).eq.t) then * This is observation is for time t - insert it. x(t,ObTypes(loop))=ActualObs(loop) loop=loop+1 else finish=.true. endif else finish=.true. endif if (.not.finish) goto 20 * Integrate one time step forward if (t.lt.Nsteps) : call Adv1Step (x,t,m1,m2,m3,l1,l2,l3,g,dt,pi,winsize) enddo * Output 'analysis' call OutputIntegration (x,Nsteps,Nwindow,AssCy,13,dt,pi, : winsize) enddo * Close the output files close (13) close (14) close (15) close (16) close (17) endif print*,'Program Completed' print*,'=========================================================' 110 format ((A),4F18.3) 111 format ((A),F10.4,(A)) 112 format (4F18.3) 113 format ((A),3F18.3) end * ================================================================== * ================================================================== subroutine Init (pi,crit,safety) * Welcome message, and initialize variables implicit none integer safety real pi,crit print*,'=========================================================' print*,'(1+1)D VAR. SYSTEM FOR THE MODIFIED DOUBLE PENDULUM' print*,'---------------------------------------------------------' print*,'Ross Bannister, Data Assimilation Research Centre' print*,' University of Reading, UK' print*,'---------------------------------------------------------' pi=4.*atan(1.) * Var. convergence conditions crit=0.001 * Limit for number of iterations/cycle safety=20 return end * ------------------------------------------------------------------ subroutine ReadConfig (Status,x0,g,dt,Nt,Nout,l1,l2,l3,m1,m2,m3, : pi,noisepos,noisevel,ActualObs,ObTypes, : ObTimes,ObErrs,Nobs,Nwindow,maxobs) * To read-in how the code is to be configured * To read-in the configuration (and observations if required) implicit none integer Nt,Nout,Nobs,maxobs,ObTypes(maxobs),Nwindow,lp real x0(4),g,dt,l1,l2,l3,m1,m2,m3,noisepos,noisevel,pi real ActualObs(maxobs),ObTimes(maxobs),ObErrs(maxobs),deg2rad character*7 Status character*10 Temp deg2rad = pi/180. * Nt : length of integration in time steps * Nout : Frequency of observation output (when creating obs) * Nobs : the number of observations * Nwindow: the size of the assimilation 'windows' in time steps print 100,'Double Pendulum 4d-Var Assimilation' print 100,'Reading in Configuration' open (12,file='Assim.conf') * Read-in general parameters read (12,100) Temp read (12,100) Temp read (12,100) Temp read (12,100) Temp read (12,100) Temp read (12,100) Status print 100,'Status : '//Status read (12,100) Temp * read (12,101) l1,l2,l3 read (12,*) l1,l2,l3 print 111, 'Lengths of arms : ',l1,l2,l3 read (12,100) Temp * read (12,101) m1,m2,m3 read (12,*) m1,m2,m3 print 111, 'Masses of nodes : ',m1,m2,m3 read (12,100) Temp * read (12,102) g read (12,*) g print 111, 'g : ',g read (12,100) Temp * read (12,102) dt read (12,*) dt print 112,'Time step : ',dt read (12,100) Temp * read (12,103) Nt read (12,103) Nt print 113,'Total No of steps: ',Nt read (12,100) Temp read (12,100) Temp * read (12,104) x0(1),x0(2),x0(3),x0(4) read (12,*) x0(1),x0(2),x0(3),x0(4) print 114,'Initial state : ',x0(1),x0(2),x0(3),x0(4) * Convert to radians and radians per second do lp=1,4 x0(lp) = deg2rad*x0(lp) enddo * Read-in parameters to do with making observations read (12,100) Temp read (12,100) Temp read (12,100) Temp * read (12,103) Nout read (12,*) Nout if (Status.eq.'MakeObs') print 113,'Ob create frq: ',Nout read (12,100) Temp * read (12,102) noisepos read (12,*) noisepos if (Status.eq.'MakeObs') print 112,'Pos. noise : ',noisepos * Convert to radians noisepos=deg2rad*noisepos read (12,100) Temp * read (12,102) noisevel read (12,*) noisevel if (Status.eq.'MakeObs') print 112,'Noise on roc : ',noisevel * Convert to radians per second noisevel=deg2rad*noisevel if (Status.ne.'MakeObs') then * Read-in parameters to do with the observations read (12,100) Temp read (12,100) Temp read (12,100) Temp * read (12,103) Nwindow read (12,*) Nwindow print 113,'Steps/cycle : ',Nwindow read (12,100) Temp read (12,100) Temp read (12,100) Temp print 100,'OBSERVATIONS :' * Loop round and read-in the observations Nobs=0 10 Nobs=Nobs+1 if (Nobs.gt.maxobs) then print*,'Insufficient space to hold observations.' print*,'Increase value of maxobs in code.' stop endif * read (12,105) ActualObs(Nobs),ObTypes(Nobs),ObErrs(Nobs), * : ObTimes(Nobs) read (12,*) ActualObs(Nobs),ObTypes(Nobs),ObErrs(Nobs), : ObTimes(Nobs) print 105,ActualObs(Nobs),ObTypes(Nobs),ObErrs(Nobs), : ObTimes(Nobs) * Convert from degrees to radians ActualObs(Nobs) = deg2rad * ActualObs(Nobs) ObErrs(Nobs) = deg2rad * ObErrs(Nobs) if ((ActualObs(Nobs).eq.0.).or.(ObTypes(Nobs).eq.0)) goto 11 goto 10 11 Nobs=Nobs-1 endif close (12) 100 format ((A)) 101 format (3F) 102 format (F) 103 format (I) 104 format (4F) 105 format (F9.4,I4,F9.4,F12.6) 111 format ((A),3F) 112 format ((A),F) 113 format ((A),I) 114 format ((A),4F) return end * ------------------------------------------------------------------ subroutine Equal (x,y,N,size) * Set the vector x=y implicit none integer size,N,i real x(size),y(size) do i=1,N x(i)=y(i) enddo return end * ------------------------------------------------------------------ subroutine PlusMinus (x,y,z,s,N,size) * x=y+sz (x,y,z, vectors) implicit none integer size,N,i real x(size),y(size),z(size),s do i=1,N x(i)=y(i)+s*z(i) enddo return end * ------------------------------------------------------------------ subroutine SetZero (x,N,size) * Set the vector x=0 implicit none integer size,N,i real x(size) do i=1,N x(i)=0. enddo return end * ------------------------------------------------------------------ subroutine MakeObs (x0,g,dt,pi,Nt,Nout,l1,l2,l3,m1,m2,m3,noisepos, : noisevel) * To run the double pendulum, outputting a set of simulated obs * x0 is the initial conditions * Nout is the frequency that the modified state is output * noise is the range of the noise distribution implicit none integer Nt,Nout,t,lp,seed,lim real x0(4),x(4),g,dt,pi,l1,l2,l3,m1,m2,m3,noisepos,noisevel,err(4) real ob,ran,ran0,rad2deg * Seed the random number generator seed=549872180 * Radians to degrees rad2deg = 180./pi * Transfer some data around err(1)=noisepos err(2)=noisepos err(3)=noisevel err(4)=noisevel * Output angular positions, and roc of angles or positions only? if (abs(noisevel).lt.(0.0000001)) then lim=2 else lim=4 endif * Put initial conditions into state vector do lp=1,4 x(lp)=x0(lp) enddo open (12,file='Observations') open (13,file='Assim.Nature') * write (12,100) '-----------------------------------------' do t=1,Nt call RungeKutt4 (x,m1,m2,m3,l1,l2,l3,g,dt,pi) write (13,102) real(t-1)*dt,rad2deg*x(1), rad2deg*x(2), : rad2deg*x(3), rad2deg*x(4) if (mod(t,Nout).eq.0) then * Make up some 'observations', lim is either 2 or 4 (see above) do lp=1,lim * Throw a random number to add noise to model value ran=ran0(seed) ob=2.*(ran-0.5)*err(lp)+x(lp) write (12,101) rad2deg*ob, lp, rad2deg*err(lp), real(t-1)*dt enddo endif enddo write (12,101) 0.,0,0.,0. * write (12,100) '-----------------------------------------' close (12) close (13) 100 format ((A)) 101 format (F9.4,I4,F9.4,F12.6) 102 format (5F16.6) return end * ------------------------------------------------------------------ subroutine FindLast (ActualObs,ObTimes,ObErrs,ObTypes,First,Last, : Index,Nobs,tfirst,tlast,dt,pi,unit,maxobs) * To find the index of the last observation in the current cycle * Also to mark the presence of each observation at the appropriate * time in the array 'index' * Also to output the observations to special files * types 1 => unit, types 2 => unit+1, * types 3 => unit+2, types 4 => unit+3 (unit is a file) implicit none integer maxobs,First,Last,Index(maxobs),Nobs,i,ObTypes(maxobs) integer unit real ActualObs(maxobs),ObTimes(maxobs),ObErrs(maxobs) real tfirst,tlast,dt,rad2deg,pi rad2deg = 180./pi * Check that the next observation is actually in this window if ((ObTimes(First).ge.tfirst).and.(ObTimes(First).lt.tlast)) then * Find extent of observations Last=First-1 10 if ((ObTimes(Last+1).ge.tlast).or.(Last.eq.Nobs)) goto 11 Last=Last+1 goto 10 11 continue * Fill-out the Index array if (Last.ge.First) then do i=First,Last Index(i)=INT((ObTimes(i)-tfirst+dt/2.)/dt) write (unit+ObTypes(i)-1,101) ObTimes(i), : rad2deg * ActualObs(i), : rad2deg * ObErrs(i) enddo endif endif 101 format (3F12.6) return end * ------------------------------------------------------------------ subroutine Advance (yt,y,Nt,i,dyi,m1,m2,m3,l1,l2,l3,g,dt,pi, : winsize) * To advance the system through Nt time steps * Element i of the initial state is incremented by dyi implicit none integer Nt,i,winsize,j,t real yt(winsize,4),dyi,m1,m2,m3,l1,l2,l3,g,dt,pi,ytemp(4),y(4) * Setup the initial state do j=1,4 yt(1,j)=y(j) ytemp(j)=y(j) enddo yt(1,i)=y(i)+dyi ytemp(i)=y(i)+dyi * Integrate over time do t=2,Nt * Advance in time call RungeKutt4 (ytemp,m1,m2,m3,l1,l2,l3,g,dt,pi) * Copy resulting state into 2d structure do j=1,4 yt(t,j)=ytemp(j) enddo enddo return end * ------------------------------------------------------------------ subroutine Adv1Step (x,t,m1,m2,m3,l1,l2,l3,g,dt,pi,winsize) * To advance the state by one time step, from t to t+1 implicit none integer t,winsize,i real x(winsize,4),m1,m2,m3,l1,l2,l3,g,dt,pi,xtemp(4) do i=1,4 xtemp(i)=x(t,i) enddo call RungeKutt4 (xtemp,m1,m2,m3,l1,l2,l3,g,dt,pi) do i=1,4 x(t+1,i)=xtemp(i) enddo return end * ------------------------------------------------------------------ subroutine Background (x0,x,AssCy,xb,Nwindow,winsize) * To set up the background state. * The background state is taken from the 'last' element of x * if AssCy>1. If AssCy=1 then take the background from x0. implicit none integer AssCy,Nwindow,winsize,i real x0(4),x(winsize,4),xb(4) if (AssCy.eq.1) then * Use x0 as background do i=1,4 xb(i)=x0(i) enddo else * Use the 'last' element of x do i=1,4 xb(i)=x(Nwindow,i) enddo endif return end * ------------------------------------------------------------------ subroutine InitialCondsInsert (x0,x,AssCy,Nwindow,winsize) * To set up the initial conditions for the 'insert' mode of the code * The initial state is taken from the 'last' element of x * if AssCy>1. If AssCy=1 then take the values from x0. implicit none integer AssCy,Nwindow,winsize,i real x0(4),x(winsize,4) if (AssCy.eq.1) then * Use x0 as initial conditions do i=1,4 x(1,i)=x0(i) enddo else * Use the 'last' element of x do i=1,4 x(1,i)=x(Nwindow,i) enddo endif return end * ------------------------------------------------------------------ subroutine GradientJOb (x,ActualObs,ObType,ObErrs,Index,GradJOb, : Jo,First,Last,pi,winsize,maxobs) * To calculate E^(-1) (obs-modelobs) and Jo implicit none integer maxobs,winsize,Index(maxobs),ObType(maxobs),First,Last,i real x(winsize,4),ActualObs(maxobs),ObErrs(maxobs),GradJOb(maxobs) real diff,pi,Jo Jo=0. do i=First,Last * Work out the difference diff=ActualObs(i)-x(Index(i),ObType(i)) * if the observation is an angle, need to do something special if (ObType(i).le.2) then * The observation - model prediction may need adjusting call AdjustDiff (diff,pi) endif GradJob(i-First+1)=-1.*diff/(ObErrs(i)*ObErrs(i)) Jo=Jo-diff*GradJob(i-First+1) enddo Jo=Jo/2. return end * ------------------------------------------------------------------ subroutine CalcJb (xB,x,Jb,Binv,winsize) * Calculation of Jb (Jo is found within subroutine GradientJOb implicit none integer winsize,i,j real xB(4),x(winsize,4),Jb,Binv(4,4),sum * Jb = tpose(xB-x(1)) Binv (xB-x(1)) Jb=0. do i=1,4 sum=0. do j=1,4 sum=sum+Binv(i,j)*(xB(j)-x(1,j)) enddo Jb=Jb+(xB(i)-x(1,i))*sum enddo Jb=Jb/2. return end * ------------------------------------------------------------------ subroutine ForwardModel (xLS,Nsteps,m1,m2,m3,l1,l2,l3,g,dt,pi, : First,Last,ObTypes,Index,MobsMinus, : MobsPlus,xt,H,winsize,maxobs) * Compute the forward model matrix dyob(j)/dxi implicit none integer i,j,Nsteps,winsize,maxobs,ObTypes(maxobs),Index(maxobs) integer First,Last real xLS(4),xt(winsize,4),delta,m1,m2,m3,l1,l2,l3,g,dt,pi real MobsMinus(maxobs),MobsPlus(maxobs),H(maxobs,4) delta=0.0001 do i=1,4 * Do two integrations by adjusting state i * Decrement element i by -delta call Advance (xt,xLS,Nsteps,i,-1.*delta,m1,m2,m3,l1,l2,l3,g,dt, : pi,winsize) * Contruct a vector of observations from this forecast call FindModelObs (MobsMinus,ObTypes,Index,xt,First,Last,maxobs, : winsize) * Increment element i by delta call Advance (xt,xLS,Nsteps,i,delta,m1,m2,m3,l1,l2,l3,g,dt,pi, : winsize) * Contruct a vector of observations from this forecast call FindModelObs (MobsPlus,ObTypes,Index,xt,First,Last,maxobs, : winsize) * Now fill out the forward model matrix do j=1,Last-First+1 H(j,i)=(MobsPlus(j)-MobsMinus(j)) if (ObTypes(j+First-1).le.2) then * The model observation difference may need adjusting call AdjustDiff (H(j,i),pi) endif H(j,i)=H(j,i)/(2.*delta) enddo enddo return end * ------------------------------------------------------------------ subroutine FindModelObs (ModelObs,ObTypes,Index,x,first,last, : maxobs,winsize) * Construct a vector of model observations implicit none integer maxobs,winsize,Index(maxobs),ObTypes(maxobs),i,first,last real ModelObs(maxobs),x(winsize,4) do i=first,last ModelObs(i-first+1)=x(Index(i),ObTypes(i)) enddo return end * ------------------------------------------------------------------ subroutine Hessian (Binv,ObErrs,H,Hess,first,last,maxobs) * To compute the Hessian in model (physical) space implicit none integer first,last,maxobs,i,j,k,l real Binv(4,4),ObErrs(maxobs),H(maxobs,4),Hess(4,4),sum do i=1,4 do j=1,4 sum=0. do k=first,last l=k-first+1 sum=sum+H(l,i)*H(l,j)/(ObErrs(k)*ObErrs(k)) enddo Hess(i,j)=Binv(i,j)+sum enddo enddo return end * ------------------------------------------------------------------ subroutine Transpose (Matrix,size) * Transpose a matrix implicit none integer size,i,j real Matrix(size,size),element do i=1,size-1 do j=i+1,size element=Matrix(i,j) Matrix(i,j)=Matrix(j,i) Matrix(j,i)=element enddo enddo return end * ------------------------------------------------------------------ subroutine GradientJTrans (GradJTrans,GradJOb,first,last,Binv,U1, : U2,xBp,H,iteration,maxobs) * To calculate the gradient of J wrt the transformed space vector implicit none integer first,last,NumObCycle,i,k,l,iteration,maxobs real GradJTrans(4),GradJOb(maxobs),Binv(4,4),U1(4,4),U2(4,4) real H(maxobs,4),xBp(4),sumb,sumo,sum,temp(4) * The number of observations in this cycle NumObCycle=last-first+1 * Stage 1: Compute -(B^-1)xBp + tpose(H)GradJOb do k=1,4 sumb=0. * If iteration=1 then know that sumb=0 as xBp=0 if (iteration.gt.1) then do l=1,4 sumb=sumb+Binv(k,l)*xBp(l) enddo endif sumo=0. do l=1,NumObCycle sumo=sumo+H(l,k)*GradJob(l) enddo temp(k)=sumo-sumb enddo * Stage 2: Compute GradJTrans (apply tpose(U) transform) do i=1,4 sum=0. do k=1,4 sum=sum+U2(k,i)*temp(k) enddo GradJTrans(i)=U1(i,i)*sum enddo return end * ------------------------------------------------------------------ subroutine Utransform (dx,dv,U1,U2,size) * Performs U-transform, dx=Udv, U = U2 U1 implicit none integer i,j,size real dx(size),dv(size),U1(size,size),U2(size,size),sum do i=1,size sum=0. do j=1,size sum=sum+U2(i,j)*U1(j,j)*dv(j) enddo dx(i)=sum enddo return end * ------------------------------------------------------------------ real function modsq (x,size) * Calculate transpose(x)(x) implicit none integer size,i real sum,x(size) sum=0. do i=1,size sum=sum+x(i)*x(i) enddo modsq=sum return end * ------------------------------------------------------------------ subroutine CalcBinv (x,Binv,Nsteps,pi,winsize) * To calculate B (time average correlation), returning its inverse implicit none integer Nsteps,winsize,i,j,k real x(winsize,4),B(4,4),Binv(4,4),Mean(4),sum,diff1,diff2,pi real Temp1(4,4),Temp2(4,4),sgn,inc * Calculate the mean value of each degree of freedom do i=1,4 sum=0. sgn=sign(1.,x(1,i)) do k=1,Nsteps inc=x(k,i) * For angles, if the sign has flipped - need to adjust inc if (i.le.2) then if (abs(sign(1.,inc)-sgn).gt.(0.1)) inc=inc+2.*pi*sgn endif sum=sum+inc enddo Mean(i)=sum/real(Nsteps) enddo * Calculate the mean correlations (B) do i=1,4 do j=1,4 sum=0. do k=1,Nsteps diff1=x(k,i)-Mean(i) if (i.le.2) call AdjustDiff (diff1,pi) diff2=x(k,j)-Mean(j) if (j.le.2) call AdjustDiff (diff2,pi) sum=sum+diff1*diff2 enddo B(i,j)=sum/real(Nsteps) enddo enddo * Invert B for Binv call Inverse (B,Binv,Temp1,Temp2,4) return end * ------------------------------------------------------------------ subroutine InvSqrt (A,size) * Invert and square root the diagonal matrix A implicit none integer i,size real A(size,size) do i=1,size A(i,i)=1./sqrt(A(i,i)) enddo return end * ------------------------------------------------------------------ subroutine OutputIntegration (xt,Nsteps,Nwindow,AssCycle,unit, : dt,pi,winsize) * To output the result of an integration implicit none integer Nsteps,Nwindow,AssCycle,unit,winsize,t real xt(winsize,4),time,dt,pi,rad2deg rad2deg = 180./pi do t=1,Nsteps time=real((AssCycle-1)*Nwindow+t-1)*dt write (unit,101) time, rad2deg*xt(t,1), : rad2deg * xt(t,2), rad2deg * xt(t,3), : rad2deg * xt(t,4) enddo 101 format (5F16.6) return end * ------------------------------------------------------------------ subroutine CheckPos (U1,size) * To check sign of eigenvalues (diagonal elements of U1) implicit none integer size,i real U1(size,size) do i=1,size if (U1(i,i).le.(0.)) then print*,' Negative eigenvalue ',U1(i,i) print*,' ABORTING CODE' stop endif enddo return end * ------------------------------------------------------------------ subroutine BoundTime (time,unit) * To output the time at the start of the boundary implicit none integer unit real time write (unit,101) time,0. 101 format (F12.6,F4.1) return end * ------------------------------------------------------------------ subroutine AdjustDiff (diff,range) * to check, adn adjust if necessary, angular ranges implicit none real diff,range if (diff.ge.range) then diff=diff-2.*range else if (diff.lt.(-1.*range)) then diff=diff+2.*range endif return end * ------------------------------------------------------------------ subroutine SetUpName (AssCy,outerloop,fname) * To concoct a filename for the output of the current integration implicit none integer AssCy,outerloop character*8 fname character*2 part1,part2 if (AssCy.lt.10) then write (part1,'(A1,I1)') '0',AssCy else write (part1,'(I2)') AssCy endif if (outerloop.lt.10) then write (part2,'(A1,I1)') '0',outerloop else write (part2,'(I2)') outerloop endif fname='Cy'//part1//'Lp'//part2 return end * ------------------------------------------------------------------ subroutine CheckHess (Hess,U1,U2) * Temporary routine to check that Hessian in trans. space = I * Also check that U2 is orthonormal implicit none integer i,j,l,m real Hess(4,4),Htrans(4,4),U1(4,4),U2(4,4),sum1,sum2 print*,' Check that U2 is orthonormal' do i=1,4 do j=1,4 sum1=0. do l=1,4 sum1=sum1+U2(l,i)*U2(l,j) enddo * Use Htrans space for convenience Htrans(i,j)=sum1 enddo print*,' U2TU2 ',(Htrans(i,j),j=1,4) enddo print*,' Check that Hess gives unit matrix' do i=1,4 do j=1,4 sum1=0. do l=1,4 sum2=0. do m=1,4 sum2=sum2+Hess(l,m)*U2(m,j) enddo sum1=sum1+U2(l,i)*sum2 enddo Htrans(i,j)=U1(i,i)*U1(j,j)*sum1 enddo print*,' H-trans2 ',(Htrans(i,j),j=1,4) enddo return end * ------------------------------------------------------------------ subroutine CheckDiag (Hess,U2) * Temporary routine to check that U2 is the evector matrix of Hess implicit none integer i,j,k,l real Hess(4,4),Htrans(4,4),U2(4,4),sum1,sum2 print*,' Check that eigenvectors' do i=1,4 do j=1,4 sum1=0. do k=1,4 sum2=0. do l=1,4 sum2=sum2+Hess(k,l)*U2(j,l) enddo sum1=sum1+U2(i,k)*sum2 enddo Htrans(i,j)=sum1 enddo print*,' H-trans1 ',(Htrans(i,j),j=1,4) enddo return end * ------ FORWARD MODEL ROUTINES ------------------------------------ subroutine matrixA (y,m1,m2,m3,l1,l2,l3,A) * To determine the elements of matrix A implicit none real y(4),m1,m2,m3,l1,l2,l3,A(2,2) real th1,th2,al1,al2,term1,term2,term3 * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=l2*l3*m3*sin(th1-th2) term2=al2*term1 term3=al1*term1 A(1,1)=-1.*term2 A(1,2)=term2 A(2,1)=-1.*term3 A(2,2)=term3 return end * ------------------------------------------------------------------ subroutine matrixBinv (y,m1,m2,m3,l1,l2,l3,Binv) * To determine the elements of matrix B-inverse implicit none real y(4),m1,m2,m3,l1,l2,l3,Binv(2,2) real B(2,2),th1,th2,al1,al2,term1,det * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=m3*l2*l3*cos(th1-th2) * Construct the actual matrix B ... B(1,1)=m1*l1*l1+(m2+m3)*l2*l2 B(1,2)=term1 B(2,1)=term1 B(2,2)=m3*l3*l3 * ... but we need its inverse det=B(1,1)*B(2,2)-B(1,2)*B(2,1) Binv(1,1)=B(2,2)/det Binv(1,2)=-1.*B(1,2)/det Binv(2,1)=-1.*B(2,1)/det Binv(2,2)=B(1,1)/det return end * ------------------------------------------------------------------ subroutine vectorf (y,m1,m2,m3,l1,l2,l3,g,f) * To determine the elements of vector f implicit none real y(4),m1,m2,m3,l1,l2,l3,g,f(2) real th1,th2,al1,al2,term1,term2 * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=m3*l2*l3*al1*al2*sin(th1-th2) term2=sin(th1) f(1)=-1.*term1+g*term2*(m1*l1-(m2+m3)*l2) f(2)=term1-g*m3*l3*sin(th2) return end * ------------------------------------------------------------------ subroutine operate (A,Binv,f,dt,y,kk) * To perform operation k=Ty+g where T(A,Binv) and g(f) * (see notes) implicit none integer i,j,k real A(2,2),Binv(2,2),f(2),dt,y(4),kk(4) real Prod(2,2),total,vec(2) * Calculate -Binv A do i=1,2 do j=1,2 total=0. do k=1,2 total=total-Binv(i,k)*A(k,j) enddo Prod(i,j)=total enddo enddo * Calculate Binv f do i=1,2 total=0. do j=1,2 total=total+Binv(i,j)*f(j) enddo vec(i)=total enddo * Now operate kk(1)=dt*y(3) kk(2)=dt*y(4) kk(3)=dt*(Prod(1,1)*y(3)+Prod(1,2)*y(4)+vec(1)) kk(4)=dt*(Prod(2,1)*y(3)+Prod(2,2)*y(4)+vec(2)) return end * ------------------------------------------------------------------ subroutine sum (a,b,c,size,flag) * Vector sum c=a+b (flag=1) * Vector sum c=a+b/2 (flag=2) implicit none integer size,i,flag real a(size),b(size),c(size) if (flag.eq.1) then do i=1,size c(i)=a(i)+b(i) enddo else do i=1,size c(i)=a(i)+b(i)/2. enddo endif return end * ------------------------------------------------------------------ subroutine RungeKutt4 (y,m1,m2,m3,l1,l2,l3,g,dt,pi) * Fourth order Runge-Kutta for double pendulum implicit none integer i real y(4),m1,m2,m3,l1,l2,l3,g,dt,pi real k1(4),k2(4),k3(4),k4(4),A(2,2),Binv(2,2),f(2),ytemp(4),mymod * Stage 1 * ------- call matrixA (y,m1,m2,m3,l1,l2,l3,A) call matrixBinv (y,m1,m2,m3,l1,l2,l3,Binv) call vectorf (y,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,y,k1) * Stage 2 * ------- call sum (y,k1,ytemp,4,2) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k2) * Stage 3 * ------- call sum (y,k2,ytemp,4,2) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k3) * Stage 4 * ------- call sum (y,k3,ytemp,4,1) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k4) * Add-up for incremented y-vector do i=1,4 y(i)=y(i) + k1(i)/6. + k2(i)/3. + k3(i)/3. + k4(i)/6. enddo * Put angles back into range (-pi to pi) do i=1,2 y(i)=mymod(y(i),pi) enddo return end * ------------------------------------------------------------------ real function mymod (x,interval) * This function puts x into the interval -interval<=xq). theta=(B(p,p)-B(q,q))/(2.0*B(q,p)) dis=sqrt(theta*theta+1) * Two possible values of t t1=-theta+dis t2=-theta-dis * Choose the smaller value if (abs(t1).lt.abs(t2)) then tt=t1 else tt=t2 endif * Find the values of s and c c=1/sqrt(tt*tt+1) s=tt*c * Modify the operator UBU(trans) (piecewise) * Update only upper-right part of matrix cc=c*c ss=s*s sc=s*c * Columns p and q: do i=1,p-1 if (i.ne.q) then Bip=B(i,p) B(i,p)=c*Bip+s*B(i,q) if (i.le.q) B(i,q)=c*B(i,q)-s*Bip endif enddo * Rows p and q: do j=q+1,N if (j.ne.p) then Bqj=B(q,j) B(q,j)=c*Bqj-s*B(p,j) if (j.gt.p) B(p,j)=c*B(p,j)+s*Bqj endif enddo * Mixed elements * B(q,p)=(cc-ss)*B(q,p)+sc*(B(q,q)-B(p,p)) B(q,p)=0.0 * New diagonal elements Bpp=B(p,p) Bqq=B(q,q) B(p,p)=cc*Bpp+ss*B(q,q)+2.0*sc*B(p,q) B(q,q)=cc*B(q,q)+ss*Bpp-2.0*sc*B(p,q) * Modified value of sum of square of off-diagonal elements... off=off-2.0*B(p,q)*B(p,q) * ...and diagonal elements diag=diag-Bpp*Bpp-Bqq*Bqq+B(p,p)*B(p,p)+B(q,q)*B(q,q) * Update lower-left of matrix do k=1,N if (k.lt.p) then B(p,k)=B(k,p) if (k.lt.q) B(q,k)=B(k,q) endif if (k.gt.q) then B(k,q)=B(q,k) if (k.gt.p) B(k,p)=B(p,k) endif enddo * Modify the transformation T(new)=U T(old) * Only rows p and q are affected do j=1,N Tpj=T(p,j) T(p,j)=c*Tpj+s*T(q,j) T(q,j)=c*T(q,j)-s*Tpj enddo endif enddo enddo sweeps=sweeps+1 if (((off/diag).gt.lim).or.(sweeps.lt.3)) goto 10 * Normalize the eigenvectors do i=1,N norm=0.0 do j=1,N norm=norm+T(i,j)*T(i,j) enddo norm=sqrt(norm) do j=1,N T(i,j)=T(i,j)/norm enddo enddo return end * ================================================================== real function ran0(idum) implicit none integer idum,IA,IM,IQ,IR,MASK real AM parameter (IA=16807,IM=2147483647,AM=1./IM,IQ=127773,IR=2836, : MASK=123459876) * Minimal random number generator of Park and Miller. Returns a * uniform random deviate between 0.0 and 1.0. Set or reset idum * to any integer value (except the unlikely value MASK) to * initialize the sequence; idum must not be altered between calls * for successive deviates in a sequence. * Numerical Recipies in Fortran, page 270 integer k idum=ieor(idum,MASK) k=idum/IQ idum=IA*(idum-k*IQ)-IR*k if (idum.lt.0) idum=idum+IM ran0=AM*idum idum=ieor(idum,MASK) return end * ==================================================================