Index: /issm/trunk/src/c/objects/Pengrid.cpp
===================================================================
--- /issm/trunk/src/c/objects/Pengrid.cpp	(revision 2222)
+++ /issm/trunk/src/c/objects/Pengrid.cpp	(revision 2223)
@@ -60,6 +60,4 @@
 	printf("   matpar_offset=%i\n",matpar_offset);
 	
-	if(node)node->Echo();
-	if(matpar)matpar->Echo();
 	return;
 }
@@ -560,4 +558,5 @@
 	int  dofs1[1]={0};
 	int  unstable=0;
+	int  reset_penalties=0;
 	
 	ParameterInputs* inputs=NULL;
@@ -579,5 +578,8 @@
 	found=inputs->Recover("temperature",&temperature,1,dofs1,numgrids,(void**)&node);
 	if(!found)throw ErrorException(__FUNCT__," could not find temperature in inputs!");
-
+	
+	found=inputs->Recover("reset_penalties",&reset_penalties);
+
+	if(reset_penalties)zigzag_counter=0;
 
 	//Compute pressure melting point
@@ -608,5 +610,5 @@
 	/*If penalty keeps zigzagging more than 5 times: */
 	if(stabilize_constraints){
-		if(zigzag_counter>5){
+		if(zigzag_counter>stabilize_constraints){
 			unstable=0;
 			active=1;
Index: /issm/trunk/src/c/parallel/control_core.cpp
===================================================================
--- /issm/trunk/src/c/parallel/control_core.cpp	(revision 2222)
+++ /issm/trunk/src/c/parallel/control_core.cpp	(revision 2223)
@@ -94,36 +94,4 @@
 		inputs->Add("fit",fit[n]);
 
-		/*Update parameters: */
-		UpdateFromInputsx(fem_model->elements,fem_model->nodes,fem_model->loads, fem_model->materials,inputs);
-
-		_printf_("%s\n","      computing gradJ...");
-		gradjcompute_results=new DataSet(ResultsEnum()); 
-		gradjcompute_core(gradjcompute_results,model, inputs);
-		gradjcompute_results->FindResult(&grad_g,"grad_g");
-		delete gradjcompute_results;
-		_printf_("%s\n","      done.");
-
-		_printf_("%s\n","      normalizing directions...");
-		Orthx(&new_grad_g,grad_g,grad_g_old);
-		VecFree(&grad_g); VecFree(&grad_g_old); 
-		grad_g_old=new_grad_g;
-		VecToMPISerial(&grad_g_double,new_grad_g);
-		_printf_("%s\n","      done.");
-
-		_printf_("%s\n","      optimizing along gradient direction...");
-		optargs.model=model;
-		optargs.param_g=param_g; optargs.grad_g=grad_g_double; optargs.inputs=inputs;optargs.n=n;
-		optpars.xmin=0; optpars.xmax=1; optpars.tolerance=tolx; optpars.maxiter=(int)maxiter[n];optpars.cmjump=cmjump[n];
-		BrentSearch(&search_scalar,J+n,&optpars,&objectivefunctionC,&optargs);
-		_printf_("%s\n","      done.");
-
-		_printf_("%s\n","      updating parameter using optimized search scalar...");
-		for(i=0;i<numberofnodes;i++)param_g[i]=param_g[i]+search_scalar*optscal[n]*grad_g_double[i];
-		_printf_("%s\n","      done.");
-
-		_printf_("%s\n","      constraining the new distribution...");    
-		ControlConstrainx(param_g,numberofnodes,mincontrolconstraint,maxcontrolconstraint,control_type);
-		_printf_("%s\n","      done.");
-
 		/*In case we are running a steady state control method, compute new temperature field using new parameter 
 		 * distribution: */
@@ -133,5 +101,39 @@
 		delete steadystate_results;
 		inputs->Add("temperature",t_g,1,numberofnodes);
-
+	
+		/*Update parameters: */
+		UpdateFromInputsx(fem_model->elements,fem_model->nodes,fem_model->loads, fem_model->materials,inputs);
+
+		_printf_("%s\n","      computing gradJ...");
+		gradjcompute_results=new DataSet(ResultsEnum()); 
+		gradjcompute_core(gradjcompute_results,model, inputs);
+		gradjcompute_results->FindResult(&grad_g,"grad_g");
+		delete gradjcompute_results;
+		_printf_("%s\n","      done.");
+
+		_printf_("%s\n","      normalizing directions...");
+		Orthx(&new_grad_g,grad_g,grad_g_old);
+		VecFree(&grad_g); VecFree(&grad_g_old); 
+		grad_g_old=new_grad_g;
+		VecToMPISerial(&grad_g_double,new_grad_g);
+		_printf_("%s\n","      done.");
+
+		_printf_("%s\n","      optimizing along gradient direction...");
+		optargs.model=model;
+		optargs.param_g=param_g; optargs.grad_g=grad_g_double; optargs.inputs=inputs;optargs.n=n;
+		optpars.xmin=0; optpars.xmax=1; optpars.tolerance=tolx; optpars.maxiter=(int)maxiter[n];optpars.cmjump=cmjump[n];
+		BrentSearch(&search_scalar,J+n,&optpars,&objectivefunctionC,&optargs);
+		_printf_("%s\n","      done.");
+
+		_printf_("%s\n","      updating parameter using optimized search scalar...");
+		for(i=0;i<numberofnodes;i++)param_g[i]=param_g[i]+search_scalar*optscal[n]*grad_g_double[i];
+		_printf_("%s\n","      done.");
+
+		_printf_("%s\n","      constraining the new distribution...");    
+		ControlConstrainx(param_g,numberofnodes,mincontrolconstraint,maxcontrolconstraint,control_type);
+		_printf_("%s\n","      done.");
+
+	
+		
 		_printf_("%s%i%s%g\n","      value of misfit J after optimization #",n+1,": ",J[n]);
 
Index: /issm/trunk/src/c/parallel/thermal_core_nonlinear.cpp
===================================================================
--- /issm/trunk/src/c/parallel/thermal_core_nonlinear.cpp	(revision 2222)
+++ /issm/trunk/src/c/parallel/thermal_core_nonlinear.cpp	(revision 2223)
@@ -58,4 +58,7 @@
 
 		if(debug)_printf_("%s\n","starting direct shooting method");
+
+		if(count==1) inputs->Add("reset_penalties",1);
+		else inputs->Add("reset_penalties",0);
 
 		/*Update parameters: */
Index: /issm/trunk/src/m/solutions/cielo/control_core.m
===================================================================
--- /issm/trunk/src/m/solutions/cielo/control_core.m	(revision 2222)
+++ /issm/trunk/src/m/solutions/cielo/control_core.m	(revision 2223)
@@ -33,4 +33,10 @@
 
 	disp(sprintf('\n%s%s%s%s\n',['   control method step ' num2str(n) '/' num2str(model.parameters.nsteps)]));
+
+	%In case we are running a steady state control method, compute new temperature field using new parameter distribution: 
+	if model.parameters.control_steady;
+		steadystate_results=steadystate_core(models,inputs); t_g=results.t_g; 
+		inputs=add(inputs,'temperature',t_g,'doublevec',1,model.parameters.numberofnodes);
+	end
 
 	%update inputs with new fit
@@ -75,10 +81,5 @@
 	param_g=ControlConstrain(param_g,model.parameters);
 
-	%In case we are running a steady state control method, compute new temperature field using new parameter distribution: 
-	if model.parameters.control_steady;
-		steadystate_results=steadystate_core(models,inputs); t_g=results.t_g; 
-		inputs=add(inputs,'temperature',t_g,'doublevec',1,model.parameters.numberofnodes);
-	end
-
+	
 	disp(['      value of misfit J after optimization #' num2str(n) ':' num2str(c(n).J)]);
 
Index: /issm/trunk/test/Verification/PigControlMethodDragP3d_22/Pig.par
===================================================================
--- /issm/trunk/test/Verification/PigControlMethodDragP3d_22/Pig.par	(revision 2222)
+++ /issm/trunk/test/Verification/PigControlMethodDragP3d_22/Pig.par	(revision 2223)
@@ -52,2 +52,4 @@
 md.maxiter=10*ones(md.nsteps,1);
 md.cmjump=0.3*ones(md.nsteps,1);
+md.debug=0;
+md.stabilize_constraints=5;
Index: /issm/trunk/test/Verification/PigControlMethodDragP3d_22/configuration.m
===================================================================
--- /issm/trunk/test/Verification/PigControlMethodDragP3d_22/configuration.m	(revision 2222)
+++ /issm/trunk/test/Verification/PigControlMethodDragP3d_22/configuration.m	(revision 2223)
@@ -26,3 +26,4 @@
 				 {'diagnostic',  'none',      0 ,     1,      'relative',    1    };...
 				 {'diagnostic',  'none',      0 ,     1,      'logarithmic', 1    };...
+				 {'steadystate',  'none',      0 ,     1,      'absolute', 1    };...
 	};
Index: /issm/trunk/test/Verification/PigControlMethodDragP3d_22/testpresolve.m
===================================================================
--- /issm/trunk/test/Verification/PigControlMethodDragP3d_22/testpresolve.m	(revision 2223)
+++ /issm/trunk/test/Verification/PigControlMethodDragP3d_22/testpresolve.m	(revision 2223)
@@ -0,0 +1,1 @@
+md.pressure=lithostaticpressure(md.rho_ice,md.g,md.surface,md.z);
