Index: mttroot/mtt/bin/trans/m/mtt_optimise.m
==================================================================
--- mttroot/mtt/bin/trans/m/mtt_optimise.m
+++ mttroot/mtt/bin/trans/m/mtt_optimise.m
@@ -24,15 +24,15 @@
   if nargin<6
     weight = ones(size(y_s));
   endif
 
   if nargin<7
-    criterion = 1e-5;
+    criterion = 1e-7;
   endif
   
   if nargin <8
-    max_iterations = 10;
+    max_iterations = 25;
   endif
 
   if nargin<9
     alpha = 1.0;
   endif
@@ -39,34 +39,60 @@
   
   if (!strcmp(method,"time"))&&(!strcmp(method,"freq"))
     error("method must be either time or freq")
   endif
   
-  N_theta = length(free);
-  Weight = weight*ones(1,N_theta); # Sensitivity weight
-  e_last = 1e20;
+  [n_data,n_y] = size(y_s);
+
+  n_th = length(free);
+  error_old = 1e20;
   error=1e10;
   theta = theta_0;
   Theta = [];
   Error = [];
   Y = [];
   iterations = -1;
-  while (abs(e_last-error)>criterion)&&(iterations<max_iterations)
+  while (abs(error_old-error)>criterion)&&(iterations<max_iterations)
     iterations = iterations + 1;
-    e_last = error;
+    error_old_old = error_old;
+    error_old = error;
     eval(sprintf("[t,y,y_theta] = mtt_s%s(system_name,theta,free);",method)); # Simulate system
+    error = 0; 
+    J = zeros(n_th,1);
+    JJ = zeros(n_th,n_th);
+    for i = 1:n_y
+      E = weight(:,i).*(y(:,i) - y_s(:,i)); # Weighted error
+      error = error + (E'*E);	# Sum the error
+
+      Weight = weight(:,i)*ones(1,n_th); # Sensitivity weight
+      y_theta_w = Weight.*y_theta(:,i:n_y:n_y*n_th); # Weighted sensitivity
+      J  = J + real(y_theta_w'*E); # Jacobian
+      JJ = JJ + real(y_theta_w'*y_theta_w); # Newton Euler approx Hessian
+    endfor
+
+    error = error
+    ## Diagnostics
+    Error = [Error error];	# Save error
     Theta = [Theta theta];	# Save parameters
     Y = [Y y];			# Save output
-    E = weight.*(y - y_s);	# Weighted error
-    y_theta = Weight.*y_theta;	# Weighted sensitivity
-    error = (E'*E);		# Sum the error
-    Error = [Error error];
-    ##    theta(free) = theta(free) - alpha*(real(y_theta'*y_theta)\real(y_theta'*E));
-    tol = 1e-4;
-    JJ = real(y_theta'*y_theta);
-    ## sigma = svd(JJ)
-    theta(free) = theta(free) - alpha*( pinv(JJ,tol)*real(y_theta'*E) );
-  endwhile
-
+
+    ## Update the estimate if we are not done yet.
+    if (abs(error_old-error)>criterion)&&(iterations<max_iterations)
+      if error>error_old+criterion # Halve step size and try again
+        factor = 10;
+	disp(sprintf("step/%i",factor));
+	error = error_old;	# Go back to previous error value
+	error_old = inf;	# Don't let it think its converged
+	theta(free) = theta(free)  + step; # Reverse step
+	step = step/factor;	# new stepsize
+      else			# Recompute step size
+	tol = 1e-5;
+	step =  pinv(JJ,tol)*J;	# Step size
+	#step =  pinv(JJ)*J;	# Step size (built in tol)
+      endif
+      theta(free) = theta(free) - step; # Increment parameters
+    endif
+
+   endwhile
 endfunction