示例#1
0
文件: ex9adj.c 项目: plguhur/petsc
int main(int argc,char **argv)
{
  TS             ts;            /* ODE integrator */
  Vec            U;             /* solution will be stored here */
  Mat            A;             /* Jacobian matrix */
  Mat            Jacp;          /* Jacobian matrix */
  PetscErrorCode ierr;
  PetscMPIInt    size;
  PetscInt       n = 2;
  AppCtx         ctx;
  PetscScalar    *u;
  PetscReal      du[2] = {0.0,0.0};
  PetscBool      ensemble = PETSC_FALSE,flg1,flg2;
  PetscReal      ftime;
  PetscInt       steps;
  PetscScalar    *x_ptr,*y_ptr;
  Vec            lambda[1],q,mu[1];

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Initialize program
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = PetscInitialize(&argc,&argv,(char*)0,help);CHKERRQ(ierr);
  ierr = MPI_Comm_size(PETSC_COMM_WORLD,&size);CHKERRQ(ierr);
  if (size > 1) SETERRQ(PETSC_COMM_WORLD,PETSC_ERR_SUP,"Only for sequential runs");

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Create necessary matrix and vectors
    - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = MatCreate(PETSC_COMM_WORLD,&A);CHKERRQ(ierr);
  ierr = MatSetSizes(A,n,n,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
  ierr = MatSetType(A,MATDENSE);CHKERRQ(ierr);
  ierr = MatSetFromOptions(A);CHKERRQ(ierr);
  ierr = MatSetUp(A);CHKERRQ(ierr);

  ierr = MatCreateVecs(A,&U,NULL);CHKERRQ(ierr);

  ierr = MatCreate(PETSC_COMM_WORLD,&Jacp);CHKERRQ(ierr);
  ierr = MatSetSizes(Jacp,PETSC_DECIDE,PETSC_DECIDE,2,1);CHKERRQ(ierr);
  ierr = MatSetFromOptions(Jacp);CHKERRQ(ierr);
  ierr = MatSetUp(Jacp);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Set runtime options
    - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,NULL,"Swing equation options","");CHKERRQ(ierr);
  {
    ctx.beta    = 2;
    ctx.c       = 10000.0;
    ctx.u_s     = 1.0;
    ctx.omega_s = 1.0;
    ctx.omega_b = 120.0*PETSC_PI;
    ctx.H       = 5.0;
    ierr        = PetscOptionsScalar("-Inertia","","",ctx.H,&ctx.H,NULL);CHKERRQ(ierr);
    ctx.D       = 5.0;
    ierr        = PetscOptionsScalar("-D","","",ctx.D,&ctx.D,NULL);CHKERRQ(ierr);
    ctx.E       = 1.1378;
    ctx.V       = 1.0;
    ctx.X       = 0.545;
    ctx.Pmax    = ctx.E*ctx.V/ctx.X;;
    ierr        = PetscOptionsScalar("-Pmax","","",ctx.Pmax,&ctx.Pmax,NULL);CHKERRQ(ierr);
    ctx.Pm      = 1.1;
    ierr        = PetscOptionsScalar("-Pm","","",ctx.Pm,&ctx.Pm,NULL);CHKERRQ(ierr);
    ctx.tf      = 0.1;
    ctx.tcl     = 0.2;
    ierr        = PetscOptionsReal("-tf","Time to start fault","",ctx.tf,&ctx.tf,NULL);CHKERRQ(ierr);
    ierr        = PetscOptionsReal("-tcl","Time to end fault","",ctx.tcl,&ctx.tcl,NULL);CHKERRQ(ierr);
    ierr        = PetscOptionsBool("-ensemble","Run ensemble of different initial conditions","",ensemble,&ensemble,NULL);CHKERRQ(ierr);
    if (ensemble) {
      ctx.tf      = -1;
      ctx.tcl     = -1;
    }

    ierr = VecGetArray(U,&u);CHKERRQ(ierr);
    u[0] = PetscAsinScalar(ctx.Pm/ctx.Pmax);
    u[1] = 1.0;
    ierr = PetscOptionsRealArray("-u","Initial solution","",u,&n,&flg1);CHKERRQ(ierr);
    n    = 2;
    ierr = PetscOptionsRealArray("-du","Perturbation in initial solution","",du,&n,&flg2);CHKERRQ(ierr);
    u[0] += du[0];
    u[1] += du[1];
    ierr = VecRestoreArray(U,&u);CHKERRQ(ierr);
    if (flg1 || flg2) {
      ctx.tf      = -1;
      ctx.tcl     = -1;
    }
  }
  ierr = PetscOptionsEnd();CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Create timestepping solver context
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSCreate(PETSC_COMM_WORLD,&ts);CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR);CHKERRQ(ierr);
  ierr = TSSetType(ts,TSRK);CHKERRQ(ierr);
  ierr = TSSetRHSFunction(ts,NULL,(TSRHSFunction)RHSFunction,&ctx);CHKERRQ(ierr);
  ierr = TSSetRHSJacobian(ts,A,A,(TSRHSJacobian)RHSJacobian,&ctx);CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Set initial conditions
   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSSetSolution(ts,U);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Save trajectory of solution so that TSAdjointSolve() may be used
   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSSetSaveTrajectory(ts);CHKERRQ(ierr);

  ierr = MatCreateVecs(A,&lambda[0],NULL);CHKERRQ(ierr);
  /*   Set initial conditions for the adjoint integration */
  ierr = VecGetArray(lambda[0],&y_ptr);CHKERRQ(ierr);
  y_ptr[0] = 0.0; y_ptr[1] = 0.0;
  ierr = VecRestoreArray(lambda[0],&y_ptr);CHKERRQ(ierr);

  ierr = MatCreateVecs(Jacp,&mu[0],NULL);CHKERRQ(ierr);
  ierr = VecGetArray(mu[0],&x_ptr);CHKERRQ(ierr);
  x_ptr[0] = -1.0;
  ierr = VecRestoreArray(mu[0],&x_ptr);CHKERRQ(ierr);
  ierr = TSSetCostGradients(ts,1,lambda,mu);CHKERRQ(ierr);
  ierr = TSSetCostIntegrand(ts,1,(PetscErrorCode (*)(TS,PetscReal,Vec,Vec,void*))CostIntegrand,
                                        (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDYFunction,
                                        (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDPFunction,PETSC_TRUE,&ctx);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Set solver options
   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSSetDuration(ts,PETSC_DEFAULT,10.0);CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_STEPOVER);CHKERRQ(ierr);
  ierr = TSSetInitialTimeStep(ts,0.0,.01);CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Solve nonlinear system
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  if (ensemble) {
    for (du[1] = -2.5; du[1] <= .01; du[1] += .1) {
      ierr = VecGetArray(U,&u);CHKERRQ(ierr);
      u[0] = PetscAsinScalar(ctx.Pm/ctx.Pmax);
      u[1] = ctx.omega_s;
      u[0] += du[0];
      u[1] += du[1];
      ierr = VecRestoreArray(U,&u);CHKERRQ(ierr);
      ierr = TSSetInitialTimeStep(ts,0.0,.01);CHKERRQ(ierr);
      ierr = TSSolve(ts,U);CHKERRQ(ierr);
    }
  } else {
    ierr = TSSolve(ts,U);CHKERRQ(ierr);
  }
  ierr = VecView(U,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = TSGetSolveTime(ts,&ftime);CHKERRQ(ierr);
  ierr = TSGetTimeStepNumber(ts,&steps);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Adjoint model starts here
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  /*   Set initial conditions for the adjoint integration */
  ierr = VecGetArray(lambda[0],&y_ptr);CHKERRQ(ierr);
  y_ptr[0] = 0.0; y_ptr[1] = 0.0;
  ierr = VecRestoreArray(lambda[0],&y_ptr);CHKERRQ(ierr);

  ierr = VecGetArray(mu[0],&x_ptr);CHKERRQ(ierr);
  x_ptr[0] = -1.0;
  ierr = VecRestoreArray(mu[0],&x_ptr);CHKERRQ(ierr);

  /*   Set RHS JacobianP */
  ierr = TSAdjointSetRHSJacobian(ts,Jacp,RHSJacobianP,&ctx);CHKERRQ(ierr);

  ierr = TSAdjointSolve(ts);CHKERRQ(ierr);

  ierr = PetscPrintf(PETSC_COMM_WORLD,"\n sensitivity wrt initial conditions: d[Psi(tf)]/d[phi0]  d[Psi(tf)]/d[omega0]\n");CHKERRQ(ierr);
  ierr = VecView(lambda[0],PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = VecView(mu[0],PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = TSGetCostIntegral(ts,&q);CHKERRQ(ierr);
  ierr = VecView(q,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = VecGetArray(q,&x_ptr);CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,"\n cost function=%g\n",(double)(x_ptr[0]-ctx.Pm));CHKERRQ(ierr);
  ierr = VecRestoreArray(q,&x_ptr);CHKERRQ(ierr);

  ierr = ComputeSensiP(lambda[0],mu[0],&ctx);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Free work space.  All PETSc objects should be destroyed when they are no longer needed.
   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = MatDestroy(&A);CHKERRQ(ierr);
  ierr = MatDestroy(&Jacp);CHKERRQ(ierr);
  ierr = VecDestroy(&U);CHKERRQ(ierr);
  ierr = VecDestroy(&lambda[0]);CHKERRQ(ierr);
  ierr = VecDestroy(&mu[0]);CHKERRQ(ierr);
  ierr = TSDestroy(&ts);CHKERRQ(ierr);
  ierr = PetscFinalize();
  return(0);
}
/*
   FormFunction - Evaluates the function and corresponding gradient.

   Input Parameters:
   tao - the Tao context
   X   - the input vector
   ptr - optional user-defined context, as set by TaoSetObjectiveAndGradientRoutine()

   Output Parameters:
   f   - the newly evaluated function
   G   - the newly evaluated gradient
*/
PetscErrorCode FormFunctionGradient(Tao tao,Vec P,PetscReal *f,Vec G,void *ctx0)
{
  TS             ts;
  PetscErrorCode ierr;
  Userctx        *ctx = (Userctx*)ctx0;
  Vec            X, F_alg;
  SNES           snes_alg;
  PetscScalar    *x_ptr;
  Vec            lambda[1];
  //Vec            q;
  Vec            mu[1];
  PetscInt       steps,steps3;
  PetscReal      t,t2;
  Vec            Xdot;
  /* FD check */
  PetscReal      f1,f2,expo;
  Vec            Pvec_eps;
  PetscReal*     P_eps;
  PetscInt i;
  PetscBool fd;
  Vec Xdist_final;

  printf("aaa\n");

  ierr  = VecGetArray(P,&x_ptr);CHKERRQ(ierr);
  H[0] = x_ptr[0];
  H[1] = x_ptr[1];
  H[2] = x_ptr[2];
  //printf("FormFunctionGradient: x=[%.14f, %.14f, %.14f]\n",  x_ptr[0],  x_ptr[1], x_ptr[2]);
  //printf("FormFunctionGradient - PD0[0]=%g\n", PD0[0]);
  ierr  = VecRestoreArray(P,&x_ptr);CHKERRQ(ierr);

  if(ctx->t0 > ctx->tdisturb) {
    printf("t0 cannot be greater than tdisturb\n");
    PetscFunctionReturn(-1);
  }
  if( (ctx->tdisturb >= ctx->trestore-1.0e-8) || (ctx->tdisturb >= ctx->tfinal-1.0e-8) ) {
    printf("tdisturb should be less than trestore and tfinal\n");
    PetscFunctionReturn(-1);
  }

  ctx->misfit=0.0;
  ctx->stepnum = 0;

  ierr = VecZeroEntries(ctx->vec_q);CHKERRQ(ierr);
  ierr = DMCreateGlobalVector(ctx->dmpgrid,&X);CHKERRQ(ierr);


  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Create timestepping solver context
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSCreate(PETSC_COMM_WORLD,&ts);CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR);CHKERRQ(ierr);
  ierr = TSSetType(ts,TSCN);CHKERRQ(ierr);
  ierr = TSSetIFunction(ts,NULL,(TSIFunction) IFunction,ctx);CHKERRQ(ierr);
  ierr = TSSetIJacobian(ts,ctx->J,ctx->J,(TSIJacobian)IJacobian,ctx);CHKERRQ(ierr);
  ierr = TSSetApplicationContext(ts,ctx);CHKERRQ(ierr);

  /* Set initial conditions */
  ierr = VecCopy(ctx->X0_disturb, X);CHKERRQ(ierr);

  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Solve from on [tdisturb, trestore] (disturbance part of the transient)
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

  /* Induce a load perturbation at t=tdisturb */
  //!for(i=0; i<3; i++) PD0[i] = PD0_disturb[i];

  /* Induce a load perturbation at t=trestore*/
  for(i=0; i<3; i++) PD0[i] = PD0_ref[i];
  //!printf("In FormFunctionGradien: Induce a load perturbance to PD0[0]=%g\n", PD0[0]);

  /* Solve for algebraic variables with Xgen given by X0_disturb */
  ierr = VecDuplicate(X,&F_alg);CHKERRQ(ierr);
  ierr = SNESCreate(PETSC_COMM_WORLD,&snes_alg);CHKERRQ(ierr);
  ierr = SNESSetFunction(snes_alg,F_alg,AlgFunction,ctx);CHKERRQ(ierr);
  ierr = MatZeroEntries(ctx->J);CHKERRQ(ierr);
  ierr = SNESSetJacobian(snes_alg,ctx->J,ctx->J,AlgJacobian,ctx);CHKERRQ(ierr);
  ierr = SNESSetOptionsPrefix(snes_alg,"alg_");CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes_alg);CHKERRQ(ierr);
  /* Solve the algebraic equations */
  ierr = SNESSolve(snes_alg,NULL,X);CHKERRQ(ierr);

  /* Just to set up the Jacobian structure */
  ierr = VecDuplicate(X,&Xdot);CHKERRQ(ierr);
  //!  ierr = IJacobian(ts,ctx->tdisturb,X,Xdot,0.0,ctx->J,ctx->J,ctx);CHKERRQ(ierr);
  ierr = IJacobian(ts,ctx->trestore,X,Xdot,0.0,ctx->J,ctx->J,ctx);CHKERRQ(ierr);
  ierr = VecDestroy(&Xdot);CHKERRQ(ierr);

  /* Save trajectory of solution so that TSAdjointSolve() may be used */
  ierr = TSSetSaveTrajectory(ts);CHKERRQ(ierr);

  /* Hook up the function evaluation */
  ierr = TSSetPostStep(ts,EvalMisfit);CHKERRQ(ierr);

  //!ierr = TSSetDuration(ts,10000,fmin(ctx->trestore,ctx->tfinal));CHKERRQ(ierr);
  ierr = TSSetDuration(ts,10000,ctx->tfinal);CHKERRQ(ierr);
  //!ierr = TSSetInitialTimeStep(ts,ctx->tdisturb,ctx->dt);CHKERRQ(ierr);
  ierr = TSSetInitialTimeStep(ts,ctx->trestore,ctx->dt);CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
  /* Solve the forward problem */
  //printf("Forward solve...\n");
  ierr = TSSolve(ts,X);CHKERRQ(ierr);

  ierr = VecDuplicate(X, &Xdist_final);CHKERRQ(ierr);
  ierr = VecCopy(X, Xdist_final);CHKERRQ(ierr);
  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Solve from on [trestore, tfinal] (post-disturbance transient)
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

  /* if(ctx->tfinal>=ctx->trestore+1.0e-8) { */
  /*   //restore  load at trestore */
  /*   for(i=0; i<3; i++) PD0[i] = PD0_ref[i]; */
    
  /*   printf("In FormFunctionGradien: Restore load to PD0[0]=%g\n", PD0[0]); */
    
  /*   /\* Solve the algebraic equations  *\/ */
  /*   ierr = SNESSolve(snes_alg,NULL,X);CHKERRQ(ierr); */
    
  /*   ierr = TSSetDuration(ts,100000,ctx->tfinal);CHKERRQ(ierr); */
  /*   ierr = TSSetInitialTimeStep(ts,ctx->trestore,ctx->dt);CHKERRQ(ierr); */
  /*   /\* Solve (from trestore to tfinal) *\/ */
  /*   ierr = TSSolve(ts,X);CHKERRQ(ierr); */
  /* } else { */
  /*   printf("Ignoring trestore since tfinal is less than it.\n"); */
  /* } */




  /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     Adjoint model starts here
     - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
  ierr = TSGetTimeStepNumber(ts,&steps3);CHKERRQ(ierr);
  ierr = TSSetPostStep(ts,NULL);CHKERRQ(ierr);

  ierr = MatCreateVecs(ctx->J,&lambda[0],NULL);CHKERRQ(ierr);

  /*   Set initial conditions for the adjoint integration */
  ierr = VecZeroEntries(lambda[0]);CHKERRQ(ierr);

  ierr = MatCreateVecs(ctx->Jacp,&mu[0],NULL);CHKERRQ(ierr);

  ierr = VecZeroEntries(mu[0]);CHKERRQ(ierr);

  /* Sets the initial value of the gradients of the cost w.r.t. x_0 and p */
  /*  Notes: the entries in these vectors must be correctly initialized */
  /* with the values lambda_i = df/dy|finaltime mu_i = df/dp|finaltime */
  ierr = TSSetCostGradients(ts,1,lambda,mu);CHKERRQ(ierr);

  /* Sets the function that computes the Jacobian of f w.r.t. p where x_t = f(x,y,p,t) */
  ierr = TSAdjointSetRHSJacobian(ts,ctx->Jacp,RHSJacobianP,ctx);CHKERRQ(ierr);

  /* Sets the routine for evaluating the integral term in the cost */
  /*ierr = TSSetCostIntegrand(ts,1,
			    (PetscErrorCode (*)(TS,PetscReal,Vec,Vec,void*))CostIntegrand,
			    (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDYFunction,
			    (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDPFunction,ctx);
  */
  ierr = TSSetCostIntegrand(ts,1,
			    NULL,
			    (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDYFunction,
			    (PetscErrorCode (*)(TS,PetscReal,Vec,Vec*,void*))DRDPFunction,ctx);
  CHKERRQ(ierr);

  t = ctx->tfinal;
  steps = (PetscInt)round(ctx->data_dt/ctx->dt);
  while(fabs(t-ctx->trestore)>1e-8)
  {
    ierr = TSGetTime(ts, &t2);CHKERRQ(ierr);

    /* Induce the perturbation in load accordingly corresponding to this time */
    if(t2-ctx->trestore>=-1e-8)
      for(i=0; i<3; i++) PD0[i] = PD0_ref[i];
    /* else if(t2-ctx->tdisturb>=0) */
    /*   for(i=0; i<3; i++) PD0[i] = PD0_disturb[i]; */
    else {printf("Panic: should not get here\n"); PetscFunctionReturn(-1);}

    /* Initial conditions for the adjoint */
    /* lambda += dr/dy */
    ierr = TSGetSolution(ts,&X);CHKERRQ(ierr);
          
    ierr = AddDRDY(t2,X,&lambda[0],ctx);CHKERRQ(ierr);
    
    //printf("Manual adjoint backward integration steps=%d t=%g t2=%g \n", steps, t, t2);
    /* Sets # steps the adjoint solver should take backward in time*/
    ierr = TSAdjointSetSteps(ts,steps);CHKERRQ(ierr);

    /* Solves the discrete adjoint problem for an ODE/DAE */
    ierr = TSAdjointSolve(ts);CHKERRQ(ierr);

    t -= steps * ctx->dt;
  }

  //printf("mu-FunctionGradient after Adjoint (t=%g)\n",t);
  //ierr = VecView(mu[0],PETSC_VIEWER_STDOUT_SELF);
  //ierr = VecView(lambda[0],PETSC_VIEWER_STDOUT_SELF);

  /* return gradient */
  ierr = VecCopy(mu[0],G);CHKERRQ(ierr);
  ierr = AddRegGradient(ctx,P,G);

  //ierr = VecView(G,PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);

  /* return fcn eval */
  *f  = ctx->misfit;
  EvalReg(ctx, P);
  *f += ctx->prior;
  //printf("objective=%.12f\n", *f);
  
  /* Finalize: destroy */
  ierr = VecDestroy(&lambda[0]);CHKERRQ(ierr);
  ierr = VecDestroy(&mu[0]);CHKERRQ(ierr);
  ierr = VecDestroy(&X);CHKERRQ(ierr);
  ierr = VecDestroy(&F_alg);CHKERRQ(ierr);
  ierr = SNESDestroy(&snes_alg);CHKERRQ(ierr);
  ierr = TSDestroy(&ts);CHKERRQ(ierr);
  //printf("Adjoint ends\n");

  fd=0;
  if(fd) {
    /* FD check */
    ierr =  FormFunction(tao,P,&f1,ctx); CHKERRQ(ierr);
    printf("cost=%.12f \n",f1);
    ierr = VecDuplicate(P, &Pvec_eps); CHKERRQ(ierr);

    for(i=0; i<3; i++) {
      for(expo=1e-2; expo>1e-8; expo/=3) {

	ierr = VecCopy(P, Pvec_eps); CHKERRQ(ierr);

	ierr = VecGetArray(Pvec_eps, &P_eps); CHKERRQ(ierr);

	P_eps[i] += expo;
	ierr = VecRestoreArray(Pvec_eps, &P_eps); CHKERRQ(ierr);

	//ierr = VecView(Pvec_eps,PETSC_VIEWER_STDOUT_SELF);

	ierr =  FormFunction(tao,Pvec_eps,&f2,ctx); CHKERRQ(ierr);
	printf("fd[%d]=%12.6e f1=%.7e f2=%.7e expo=%g\n", i+1, (f2-f1)/expo, f1, f2, expo);
      }
    }
    ierr = VecDestroy(&Pvec_eps); CHKERRQ(ierr); 
    /* ~end of FD */
  }
  //PetscFunctionReturn(-1);
  PetscFunctionReturn(0);
}