Added vertical averaging

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

@@ -118,6 +118,7 @@ import com.elphel.imagej.gpu.JCuda_ImageJ_Example_Plugin;
 import com.elphel.imagej.ims.DjiSrt;
 import com.elphel.imagej.ims.DjiSrtReader;
 import com.elphel.imagej.ims.EventLogger;
+import com.elphel.imagej.ims.QuatVertLMA;
 import com.elphel.imagej.ims.UasLogReader;
 //import com.elphel.imagej.ims.Imx5;
 import com.elphel.imagej.jp4.JP46_Reader_camera;
@@ -6164,11 +6165,24 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 				System.out.println("Failed to create eventLogger instance.");
 				return;
 			}
+			double [][] quats_all = eventLogger.getQuats(
+					CLT_PARAMETERS.imp.imsv_ts0, //  1763232236.0, // double ts_start, or NaN
+					CLT_PARAMETERS.imp.imsv_ts1); // 1763234000.0); // double ts_end)
+			double [] vert_xyz = null;
+			if (quats_all != null) {
+				vert_xyz = QuatVertLMA.getVertAndRms(
+						CLT_PARAMETERS, // CLTParameters   clt_parameters,
+						quats_all,      // double [][] quats,
+						DEBUG_LEVEL);   // int             debugLevel)
+			}
+			
 			eventLogger.print2Pps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"rec_1pps");
 			eventLogger.printStrobeInTime(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_strobe_in_time");
 			eventLogger.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");
 			eventLogger.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);
-			eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1");
+			eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1",
+					vert_xyz); // may be null. 4-element - ignore last (it is RMS)
+			
 			eventLogger.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");
  /*
 			if (EVENT_LOGGER == null) {

src/main/java/com/elphel/imagej/ims/EventLogger.java

@@ -87,7 +87,7 @@ public class EventLogger {
 		
 	}
 	
-	public void printDidIns1(String out_did1_path) {
+	public void printDidIns1(String out_did1_path, double [] vert_xyz) {
 		out_did1_path = Paths.get(out_did1_path).normalize().toString();
 		Path op_did1 = Paths.get(out_did1_path);
 		op_did1=op_did1.normalize();
@@ -97,7 +97,7 @@ public class EventLogger {
 			if (logger_files[nf].hasIns(false)) {
 				System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
 				try {
-					logger_files[nf].listDidIns1(out_path,false);
+					logger_files[nf].listDidIns1(out_path,false, vert_xyz);
 				} catch (IOException e) {
 					// TODO Auto-generated catch block
 					e.printStackTrace();
@@ -131,6 +131,60 @@ public class EventLogger {
 		}
 	}
 	
+	public double [][] getQuats(
+			double ts_start,
+			double ts_end){
+		if (ts_start < 0) ts_start=Double.NaN;
+		if (ts_end   < 0) ts_end=Double.NaN;
+		double [][] quats_all = null;
+		double [][] quats =    null;
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			quats = null;
+			if (logger_files[nf].hasIns(true)) {
+				System.out.println("Processing all DID_INS_2 data in "+logger_files[nf].abs_path);
+				try {
+					quats = logger_files[nf].getQuats(
+							ts_start, // double ts_start, // or NaN
+							ts_end,   // double ts_end, // or NaN
+							false);   // boolean keep) throws IOException {
+
+				} catch (IOException e) {
+					// TODO Auto-generated catch block
+					e.printStackTrace();
+				}
+			} else {
+				System.out.println("DID_INS_2 data is not available in "+logger_files[nf].abs_path);
+			}
+			if ((quats == null) || (quats.length ==0)) {
+				System.out.println("Processing all DID_INS_1 data in "+logger_files[nf].abs_path);
+				try {
+					quats = logger_files[nf].getQuatsIns1(
+							ts_start, // double ts_start, // or NaN
+							ts_end,   // double ts_end, // or NaN
+							false);   // boolean keep) throws IOException {
+
+				} catch (IOException e) {
+					// TODO Auto-generated catch block
+					e.printStackTrace();
+				}
+			}
+			if ((quats != null) && (quats.length > 0)) {
+				if (quats_all == null) {
+					quats_all = quats;
+				} else {
+					double [][] quats0 = quats_all;
+					quats_all = new double[quats0.length + quats.length][];
+					System.arraycopy(quats0, 0, quats_all, 0,             quats0.length);
+					System.arraycopy(quats,  0, quats_all, quats0.length, quats.length);
+				}
+			}
+		}		
+		return quats_all;
+	}
+	
+	
+	
+	
 	public void printPimu(String out_pimu_path) {
 		out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
 		Path op_pimu = Paths.get(out_pimu_path);

src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java

@@ -751,7 +751,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 	}
 	
 	
-	public void listDidIns1(String path, boolean keep) throws IOException {
+	public void listDidIns1(String path, boolean keep, double [] vert_xyz) throws IOException {
 		double max_offset = 1000; // m
 		open();
 		setInsQuats();
@@ -772,7 +772,11 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 
 		        "u\tv\tw\t"+
 		        "lat\tlong\talt\t"+
-		        "ned:N\tned:E\tned:D\n"; 
+		        "ned:N\tned:E\tned:D"; 
+		if (vert_xyz != null) {
+			header +="\tvert:X\tvert:Y\tvert:Z";
+		}
+		header +="\n";
 		if (writer != null) {
 			writer.write(header);
 		} else {
@@ -813,7 +817,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 				String line =String.format(
 						"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+
 						"%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
-						"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f\n",
+						"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f",
 						nrec,
 						getMasterTS(nrec),
 						getLocalTS(nrec),
@@ -826,6 +830,18 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 						did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2],       //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
 						did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2]        //  North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude
 						);
+				if (vert_xyz != null) {
+					double [] quat = {ins_quats[nrec][0],ins_quats[nrec][1],ins_quats[nrec][2],ins_quats[nrec][3]};
+					double [][] R = QuatVertLMA.quaternionToRotationMatrix(quat);
+					double [] ims_down = new double[3];
+					for (int i = 0; i < 3; i++) {
+						for (int k = 0; k < 3; k++) {
+							ims_down[i] += R[i][k]*vert_xyz[k];
+						}
+					}
+					line +=String.format("\t%13.10f\t%13.10f\t%13.10f",ims_down[0],ims_down[1],ims_down[2]);
+				}
+				line += "\n";
 				if (writer != null) {
 					writer.write(line);
 				} else {
@@ -911,6 +927,129 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		}
 	}
 	
+	
+	public double [][] getQuats(
+			double ts_start, // or NaN
+			double ts_end, // or NaN
+			boolean keep) throws IOException {
+		class Double4 {
+			double [] quat = new double[4];
+			Double4 (float [] fdata){
+				double [] data = new double[fdata.length];
+				for (int i = 0; i < data.length; i++) {
+					data[i] = fdata[i];
+				}
+			}
+		}
+		ArrayList<Double4> quat_list = new ArrayList<Double4>();
+		open();
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("listDidIns2(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
+				if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
+					continue;
+				}
+				double ts = getMasterTS(nrec);
+				if (!(ts < ts_start) && !(ts > ts_end)) {
+					quat_list.add(new Double4(did_ins_2.qn2b));
+				}
+			}
+		}
+		if (!keep) {
+			close();
+		}
+		double [][] quats = new double[quat_list.size()][];
+		for (int i = 0; i < quats.length; i++) {
+			quats[i] = quat_list.get(i).quat;
+		}
+		return quats;
+	}
+	
+	
+	
+	public double [][] getQuatsIns1(
+			double ts_start, // or NaN
+			double ts_end, // or NaN
+			boolean keep) throws IOException {
+		class Double4 {
+			double [] quat;
+			Double4 (float [] fdata){
+				quat = new double[fdata.length];
+				for (int i = 0; i < quat.length; i++) {
+					quat[i] = fdata[i];
+				}
+				return;
+			}
+		}
+		ArrayList<Double4> quat_list = new ArrayList<Double4>();
+		double max_offset = 1000; // m
+		open();
+		setInsQuats();
+		float [] start_ned = null;
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("getQuatsIns1(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
+				if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
+					continue;
+				}
+				if ((start_ned != null) &&
+						(       (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) ||
+								(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) ||
+								(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
+								)){
+					start_ned = null;
+				}
+				if (start_ned == null) {
+					start_ned = did_ins_1.ned.clone();
+				}
+				start_ned = new float[] {0,0,0};
+				
+				double ts = getMasterTS(nrec);
+				if (!(ts < ts_start) && !(ts > ts_end)) {
+					quat_list.add(new Double4(ins_quats[nrec]));
+				}
+			}
+		}
+		if (!keep) {
+			close();
+		}
+		double [][] quats = new double[quat_list.size()][];
+		for (int i = 0; i < quats.length; i++) {
+			quats[i] = quat_list.get(i).quat;
+		}
+		return quats;
+	}
+	
 	public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
 		// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
 //		double max_offset = 1000; // m

src/main/java/com/elphel/imagej/ims/QuatVertLMA.java

+package com.elphel.imagej.ims;
+
+import java.util.Arrays;
+import java.util.concurrent.atomic.AtomicInteger;
+
+import com.elphel.imagej.cameras.CLTParameters;
+import com.elphel.imagej.tileprocessor.ImageDtt;
+
+import Jama.Matrix;
+
+/// Find average vertical vector, relative to IMS using array of quaternions relative to NED
+public class QuatVertLMA {
+	public static final int [] NED_DOWN =      {0,0,1};
+	private int               N =               0;
+	private int               samples =         0;
+	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
+	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
+	private double []         initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
+	private double []         parameters_vector = null;
+	private double [][]       ims_quats =       null;
+//	private double []         x_vector =        null;
+	private double []         y_vector =        null;
+//	private double []         y_inv_vector =    null;
+	private double []         weights;          // normalized so sum is 1.0 for all - samples and extra regularization
+	private double            pure_weight;      // weight of samples only	
+	private double []         last_ymfx =       null;
+	private double [][]       last_jt =         null;
+	
+	public double [] getLastRms() {
+		return last_rms;
+	}
+	
+	public double [] getInitialRms() {
+		return initial_rms;
+	}
+	
+	public double [] getVector() {
+		return parameters_vector;
+	}
+	
+	public static double [] getVertAndRms(
+			CLTParameters   clt_parameters,
+			double [][]     quats,
+			int             debugLevel) {
+		debugLevel+=3;
+		QuatVertLMA quatVertLMA = new QuatVertLMA();
+		double [] xyz0 = {0,0,1};
+		quatVertLMA.prepareLMA(
+				quats, // double [][] quats,
+				xyz0); // double []   xyz0)
+		int OK = quatVertLMA.runLma(                      // <0 - failed, >=0 iteration number (1 - immediately)
+				clt_parameters.imp.imsv_lambda,           // double lambda,           // 0.1
+				clt_parameters.imp.imsv_lambda_scale_good,// double lambda_scale_good,// 0.5
+				clt_parameters.imp.imsv_lambda_scale_bad, // double lambda_scale_bad, // 8.0
+				clt_parameters.imp.imsv_lambda_max,       // double lambda_max,       // 100
+				clt_parameters.imp.imsv_rms_diff,         // double rms_diff,         // 0.001
+				clt_parameters.imp.imsv_num_iter,         // int    num_iter,         // 20
+				debugLevel);                              // int    debug_level)
+		if (OK < 0) {
+			return null;
+		}
+		double [] xyz = quatVertLMA.getVector();
+		double rms = quatVertLMA.getLastRms()[0];
+		if (debugLevel > -3) {
+			System.out.println("getVertAndRms{}: calculated vertical vector: ["+xyz[0]+", "+xyz[1]+", "+xyz[2]+"], RMSE="+rms);
+		}
+		if (clt_parameters.imp.imsv_calc) {
+			clt_parameters.imp.imsv_xyz = xyz;
+			if (debugLevel > -3) {
+				System.out.println("getVertAndRms{}: saved vertical vector to parameters (imsv_xyz): ["+xyz[0]+", "+xyz[1]+", "+xyz[2]+"]");
+			}
+		}
+		return new double [] {xyz[0],xyz[1],xyz[2],rms};
+	}
+	
+	
+	public void prepareLMA(
+			double [][] quats,
+			double []   xyz0) {
+		N = NED_DOWN.length;
+		samples = quats.length;
+		ims_quats = quats;
+		parameters_vector = xyz0;
+		y_vector = new double[N * samples];
+		weights = new double[y_vector.length];
+		Arrays.fill(weights, 1.0/weights.length);
+		for (int n = 0; n < ims_quats.length; n++) {
+			for (int i = 0; i < N; i++) {
+				y_vector[N * n + i] = NED_DOWN[i];
+			}
+		}
+		last_jt = new double [parameters_vector.length][];
+		return; 
+	}
+	
+	
+	private double compareJT(
+			double [] vector,
+			double    delta) {
+		int num_dbg_lines = 300;
+		double []  errors=new double [vector.length];
+		double [][] jt =  new double [vector.length][];
+//		System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
+		System.out.print("Parameters vector = [");
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(vector[i]);
+			if (i < (vector.length -1)) System.out.print(", ");
+		}
+		System.out.println("]");
+		getFxDerivs(
+				vector,
+				jt, // final double [][] jt, // should be null or initialized with [vector.length][]
+				1); // debug_level);
+		double [][] jt_delta =  getFxDerivsDelta(
+				vector, // double []         vector,
+				delta, // final double      delta,
+				-1); // final int         debug_level)
+		System.out.print(String.format("%5s","index"));
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(String.format("\t%12s","jt["+i+"]"));
+		}			
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(String.format("\t%12s","jt_delta["+i+"]"));
+		}			
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(String.format("\t%12s","jt_diff["+i+"]"));
+		}			
+		System.out.println();
+
+		for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {
+			if (n < num_dbg_lines) {
+				System.out.print(String.format("%5d",n));
+				for (int i = 0; i < vector.length; i++) {
+					System.out.print(String.format("\t%12.9f",jt[i][n]));
+				}			
+				for (int i = 0; i < vector.length; i++) {
+					System.out.print(String.format("\t%12.9f",jt_delta[i][n]));
+				}			
+				for (int i = 0; i < vector.length; i++) {
+					System.out.print(String.format("\t%12.9f",jt[i][n]-jt_delta[i][n]));
+				}			
+				System.out.println();
+			}
+			/*
+			System.out.println(String.format(
+					"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
+					n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],
+					jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],
+					jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
+					*/
+			for (int i = 0; i < vector.length; i++) {
+				errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
+			}
+		}
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print("\t\t");
+		}			
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(String.format("\t%12.9f",errors[i]));
+		}			
+        /*
+		System.out.println(String.format(
+				"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
+				errors[0], errors[1], errors[2], errors[3]));
+				*/
+		double err=0;
+		for (int i = 0; i < vector.length; i++) {
+			err = Math.max(errors[i], err);
+		}
+		return err;
+	}
+	
+	
+	
+	private double [][] getFxDerivsDelta(
+			double []         vector,
+			final double      delta,
+			final int         debug_level) {
+		double [][] jt =  new double [vector.length][weights.length];
+		for (int nv = 0; nv < vector.length; nv++) {
+			double [] vpm = vector.clone();
+			vpm[nv]+= 0.5*delta;
+			double [] fx_p =  getFxDerivs(
+					vpm,
+					null, // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);
+			vpm[nv]-= delta;
+			double [] fx_m =  getFxDerivs(
+					vpm,
+					null, // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);
+			for (int i = 0; i < weights.length; i++) if (weights[i] > 0) {
+				jt[nv][i] = (fx_p[i]-fx_m[i])/delta;
+			}
+		}
+		return jt;
+	}
+
+	
+	
+	private double [] getFxDerivs(
+			double []         vector,
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		double [] fx = new double [weights.length];
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+		}
+		/*
+		for (int n = 0; n < samples; n++) {
+			double [] ims_down = applyTo(
+					ims_quats[n], // double [] q,
+					vector); // double [] xyz)
+			System.arraycopy(ims_down, 0, fx, vector.length * n, vector.length);
+		}
+		*/
+		for (int n = 0; n < samples; n++) {
+			double [][] R = quaternionToRotationMatrix(
+					ims_quats[n]); // double [] q,
+			double [] ims_down = new double[3];
+			for (int i = 0; i < 3; i++) {
+				for (int k = 0; k < 3; k++) {
+					ims_down[i] += R[i][k]*vector[k];
+				}
+			}
+			System.arraycopy(ims_down, 0, fx, vector.length * n, vector.length);
+
+			if (jt != null) {
+				for (int i = 0; i < vector.length; i++) {
+					for (int j = 0; j < vector.length; j++) {
+						jt[i][vector.length * n + j] = R[j][i]; 
+					}
+				}
+			}
+		}			
+		
+		/*
+		
+		for (int n = 0; n < samples; n++) {
+			double [] ims_down = applyTo(
+					ims_quats[n], // double [] q,
+					vector); // double [] xyz)
+			System.arraycopy(ims_down, 0, fx, vector.length * n, vector.length);
+		}
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+			for (int n = 0; n < samples; n++) {
+				double [][] derivs = quaternionToRotationMatrix(
+						ims_quats[n]); // double [] q,
+				for (int i = 0; i < vector.length; i++) {
+					for (int j = 0; j < vector.length; j++) {
+						jt[i][vector.length * n + j] =derivs[j][i]; 
+					}
+				}				
+			}			
+		}
+		*/
+		return fx;
+	}
+	
+	private double [] getYminusFxWeighted(
+			final double []   fx,
+			final double []   rms_fp // null or [2]
+			) {
+		final double []     wymfw =       new double [fx.length];
+		double s_rms=0; 
+		double rms_pure=Double.NaN;
+		for (int i = 0; i < fx.length; i++) {
+			double d = y_vector[i] - fx[i];
+			double wd = d * weights[i];
+			if (Double.isNaN(wd)) {
+				System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
+						", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
+				wd = 0.0;
+				d = 0.0;
+			}
+			if (i == (samples * N)) {
+				rms_pure = Math.sqrt(s_rms/pure_weight);;	
+			}
+			wymfw[i] = wd;
+			s_rms += d * wd;
+		}
+		double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
+		if (Double.isNaN(rms_pure)) {
+			rms_pure=rms;
+		}
+		if (rms_fp != null) {
+			rms_fp[0] = rms;
+			rms_fp[1] = rms_pure;
+		}
+		return wymfw;
+	}
+	
+	private double [][] getWJtJlambda( // USED in lwir
+			final double      lambda,
+			final double [][] jt)
+	{
+		final int num_pars = jt.length;
+		final int num_pars2 = num_pars * num_pars;
+		final int nup_points = jt[0].length;
+		final double [][] wjtjl = new double [num_pars][num_pars];
+		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
+						int i = indx / num_pars;
+						int j = indx % num_pars;
+						if (j >= i) {
+							double d = 0.0;
+							for (int k = 0; k < nup_points; k++) {
+								if (jt[i][k] != 0) {
+									d+=0;
+								}
+								d += weights[k]*jt[i][k]*jt[j][k];
+							}
+							wjtjl[i][j] = d;
+							if (i == j) {
+								wjtjl[i][j] += d * lambda;
+							} else {
+								wjtjl[j][i] = d;
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return wjtjl;
+	}
+	
+	public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+			double lambda,           // 0.1
+			double lambda_scale_good,// 0.5
+			double lambda_scale_bad, // 8.0
+			double lambda_max,       // 100
+			double rms_diff,         // 0.001
+			int    num_iter,         // 20
+			int    debug_level) {
+		boolean [] rslt = {false,false};
+		this.last_rms = null; // remove?
+		int iter = 0;
+		for (iter = 0; iter < num_iter; iter++) {
+			rslt =  lmaStep(
+					lambda,
+					rms_diff,
+					debug_level);
+			if (rslt == null) {
+				return -1; // false; // need to check
+			}
+			if (debug_level > 1) {
+				System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
+						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda+
+						", xyz=["+parameters_vector[0]+","+parameters_vector[1]+","+parameters_vector[2]+"]");
+			}
+			if (rslt[1]) {
+				break;
+			}
+			if (rslt[0]) { // good
+				lambda *= lambda_scale_good;
+			} else {
+				lambda *= lambda_scale_bad;
+				if (lambda > lambda_max) {
+					break; // not used in lwir
+				}
+			}
+		}
+		if (rslt[0]) { // better
+			if (iter >= num_iter) { // better, but num tries exceeded
+				if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
+			}
+
+		} else { // improved over initial ?
+			if (last_rms[0] < initial_rms[0]) { // NaN
+				rslt[0] = true;
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
+			}
+		}
+		boolean show_intermediate = true;
+		if (show_intermediate && (debug_level > 0)) {
+			System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+		}
+		/*
+		if (debug_level > 2){ 
+			String [] lines1 = printOldNew(false); // boolean allvectors)
+			System.out.println("iteration="+iter);
+			for (String line : lines1) {
+				System.out.println(line);
+			}
+		}
+		if (debug_level > 0) {
+			if ((debug_level > 1) ||  last_run) { // (iter == 1) || last_run) {
+				if (!show_intermediate) {
+					System.out.println("LMA: iter="+iter+",   full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+				}
+				String [] lines = printOldNew(false); // boolean allvectors)
+				for (String line : lines) {
+					System.out.println(line);
+				}
+			}
+		}
+		if ((debug_level > -2) && !rslt[0]) { // failed
+			if ((debug_level > 1) || (iter == 1) || last_run) {
+				System.out.println("LMA failed on iteration = "+iter);
+				String [] lines = printOldNew(true); // boolean allvectors)
+				for (String line : lines) {
+					System.out.println(line);
+				}
+			}
+			System.out.println();
+		}
+		if (debug_level > 0) {
+			double [] fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					null,              // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			debugYfX ( "fx-",   // String pfx,
+					fx); // double [] data)
+			if (debug_level > 2) {
+				debugYfX ( "ffx-",   // String pfx,
+						dbg_data); // double [] data)
+			}
+			if (debug_level > 1) {
+				double    delta = 1E-5;
+				System.out.println("\n\n");
+				double err = compareJT(
+						parameters_vector, // double [] vector,
+						delta);            // double    delta);
+				System.out.println("Maximal error = "+err);
+			}
+
+		}
+		*/
+		
+		if (debug_level > 2) {
+			double    delta = 1E-5;
+			System.out.println("\n\n");
+			double err = compareJT(
+					parameters_vector, // double [] vector,
+					delta);            // double    delta);
+			System.out.println("Maximal error = "+err);
+		}
+		return rslt[0]? iter : -1;
+	}
+	
+
+	
+	
+	private boolean [] lmaStep(
+			double lambda,
+			double rms_diff,
+			int debug_level) {
+		boolean [] rslt = {false,false};
+		// maybe the following if() branch is not needed - already done in prepareLMA !
+		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
+			last_rms = new double[2];
+			if (debug_level > 1) {
+				System.out.println("lmaStep(): first step");
+			}
+			double [] fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			/*
+			if (debug_level > 0) {
+				 debugYfX ( "fx0-",   // String pfx,
+						 fx); // double [] data)
+			}
+			if (debug_level > 2) {
+				debugYfX ( "ffx0-",   // String pfx,
+						dbg_data); // double [] data)
+			}
+			*/
+			if (debug_level > 2) {
+				double    delta = 1E-5;
+				System.out.println("\n\n");
+				double err = compareJT(
+						parameters_vector, // double [] vector,
+						delta);            // double    delta);
+				System.out.println("Maximal error = "+err);
+			}
+			
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					last_rms); // final double []   rms_fp // null or [2]
+			this.initial_rms = this.last_rms.clone();
+			this.good_or_bad_rms = this.last_rms.clone();
+
+			if (debug_level > -1) { // temporary
+				/*
+				dbgYminusFxWeight(
+						this.last_ymfx,
+						this.weights,
+						"Initial_y-fX_after_moving_objects");
+                */
+			}
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			// TODO: Restore/implement
+			if (debug_level > 3) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+				*/
+			}
+		}
+		Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
+
+		Matrix wjtjlambda = new Matrix(getWJtJlambda(
+				lambda, // *10, // temporary
+				this.last_jt)); // double [][] jt) // null
+		
+		if (debug_level>2) {
+			System.out.println("JtJ + lambda*diag(JtJ");
+			wjtjlambda.print(18, 10);
+		}
+		Matrix jtjl_inv = null;
+		try {
+			jtjl_inv = wjtjlambda.inverse(); // check for errors
+		} catch (RuntimeException e) {
+			rslt[1] = true;
+			if (debug_level > 0) {
+				System.out.println("Singular Matrix!");
+			}
+
+			return rslt;
+		}
+		if (debug_level>2) {
+			System.out.println("(JtJ + lambda*diag(JtJ).inv()");
+			jtjl_inv.print(18, 10);
+		}
+//last_jt has NaNs
+		Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
+		if (debug_level>2) {
+			System.out.println("Jt * (y-fx)");
+			jty.print(18, 10);
+		}
+		
+		
+		Matrix mdelta = jtjl_inv.times(jty);
+		if (debug_level>2) {
+			System.out.println("mdelta");
+			mdelta.print(18, 10);
+		}
+
+		double scale = 1.0;
+		double []  delta =      mdelta.getColumnPackedCopy();
+		double []  new_vector = parameters_vector.clone();
+		for (int i = 0; i < parameters_vector.length; i++) {
+			new_vector[i] += scale * delta[i];
+		}
+		
+		
+		double [] fx = getFxDerivs(
+				new_vector, // double []         vector,
+				last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+				debug_level);      // final int         debug_level)
+		double [] rms = new double[2];
+		last_ymfx = getYminusFxWeighted(
+				fx, // final double []   fx,
+				rms); // final double []   rms_fp // null or [2]
+		if (debug_level > 2) {
+			/*
+			dbgYminusFx(this.last_ymfx, "next y-fX");
+			dbgXY(new_vector, "XY-correction");
+			*/
+		}
+
+		if (last_ymfx == null) {
+			return null; // need to re-init/restart LMA
+		}
+
+		this.good_or_bad_rms = rms.clone();
+		if (rms[0] < this.last_rms[0]) { // improved
+			rslt[0] = true;
+			rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
+			this.last_rms = rms.clone();
+
+			this.parameters_vector = new_vector.clone();
+			if (debug_level > 2) {
+				// print vectors in some format
+				/*
+				System.out.print("delta: "+corr_delta.toString()+"\n");
+				System.out.print("New vector: "+new_vector.toString()+"\n");
+				System.out.println();
+				*/
+			}
+		} else { // worsened
+			rslt[0] = false;
+			rslt[1] = false; // do not know, caller will decide
+			// restore state
+			fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					this.last_rms); // final double []   rms_fp // null or [2]
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			if (debug_level > 2) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+							*/
+			}
+		}
+		return rslt;
+	}
+
+	
+	
+	
+	
+	
+	public static double vecLen(double [] xyz) {
+		double l2 = 0;
+		for (int i = 0; i < xyz.length; i++) l2 += xyz[i] * xyz[i];
+		return Math.sqrt(l2);
+	}
+	
+	/**
+	 * Normalize vector xyz
+	 * @param xyz
+	 * @return
+	 */
+	public static double [] normVect(double [] xyz) {
+		int n = xyz.length;
+		double l = vecLen(xyz);
+		double [] xyzn = new double [n];
+		for (int i = 0; i < n; i++) xyzn[i] = xyz[i]/l;
+		return xyzn;
+	}
+	
+	/**
+	 * Get derivatives of normalized x,y,z by x,y,z 
+	 * @param xyz x,y,z before normalization
+	 * @return rows - normalized indices, columns - xyz. Matrix multiplication of the result
+	 * by the column {{dx},{dy},{dz}} will result in column {{dxn},{dyn},{dzn}} 
+	 */
+	public static double [][] dNorm(double [] xyz){
+		int n = xyz.length;
+		double l = vecLen(xyz);
+		double l2 = l*l;
+		double l32 = l*l2;
+		double [][] dn_d = new double[n][n];
+		for (int i = 0; i < n; i++) {
+			for (int j = 0; j <n; j++) {
+				if (j==i) {
+					dn_d[j][j] = (l2 - xyz[i]*xyz[i])/l32;
+				} else {
+					dn_d[j][j] = -xyz[i]*xyz[j]/l32;
+				}
+			}
+		}
+		return dn_d;
+	}
+	
+	/**
+	 * Apply quaternion to a 3D vector 
+	 * @param q   4 components (scalar, vector) of the quaternion being applied
+	 *            to as vector.
+	 * @param  xyz 1-d array representing a 3D vector {X, Y, Z}
+	 * @return rotated 3D vector as 1 1D array {X, Y, Z}   
+	 */
+	public static double [] applyTo(
+			double [] q,
+			double [] xyz) {
+		final double s = q[1] * xyz[0] + q[2] * xyz[1] + q[3] * xyz[2];
+		return new double [] {
+				2 * (q[0] * (xyz[0] * q[0] - (q[2] * xyz[2] - q[3] * xyz[1])) + s * q[1]) - xyz[0],
+				2 * (q[0] * (xyz[1] * q[0] - (q[3] * xyz[0] - q[1] * xyz[2])) + s * q[2]) - xyz[1],
+				2 * (q[0] * (xyz[2] * q[0] - (q[1] * xyz[1] - q[2] * xyz[0])) + s * q[3]) - xyz[2]};
+	}
+	
+	/**
+	 * Derivative of the result of the applyTo() by each of the xyz component. It is the same as the quaternion
+	 * converted to a rotation matrix so obviously does not depend on the vector .
+	 *     |  w^2+qx^2-qy^2-qz^2, 2*(qx*qy-qz*w),     2*(qx*qz+qy*w)     |
+     * R = |  2*(qx*qy+qz*w),     w^2-qx^2+qy^2-qz^2, 2*(qy*qz-qx*w)     |
+     *     |  2*(qx*qz-qy*w),     2*(qy*qz+qx*w),     w^2-qx^2-qy^2+qz^2 |
+	 * @param q   4 components (w, qx, qy, qz) of the quaternion
+	 * @return Jacobian - derivatives of the rotated vector by each of the xyz vector components. rows - components of the rotated vector,
+	 * columns - input vector components.
+	 */
+	public static double [][] quaternionToRotationMatrix(
+			double [] q) {
+		double q00 = q[0]*q[0];
+		double q11 = q[1]*q[1];
+		double q22 = q[2]*q[2];
+		double q33 = q[3]*q[3];
+		
+		double q01 = q[0]*q[1];
+		double q02 = q[0]*q[2];
+		double q03 = q[0]*q[3];
+		double q12 = q[1]*q[2];
+		double q13 = q[1]*q[3];
+		double q23 = q[2]*q[3];
+		
+		return new double [][]{
+			{	q00+q11-q22-q33, 2*(q12-q03),     2*(q13+q02)},
+			{	2*(q12+q03),     q00-q11+q22-q33, 2*(q23-q01)},
+			{	2*(q13-q02),     2*(q23+q01),     q00-q11-q22+q33}};
+	}
+	
+
+	/**
+	 * Apply quaternion q to quaternion r
+	 * @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
+	 * @param r - 4 components (scalar, vector) of the target quaternion to which to apply the first one
+	 * @return composed quaternion
+	 */
+	public static double [] compose(
+			double [] q,
+			double [] r) {
+		return normSign(new double [] {
+				r[0] * q[0] - (r[1] * q[1] +  r[2] * q[2] + r[3] * q[3]),
+				r[1] * q[0] +  r[0] * q[1] + (r[2] * q[3] - r[3] * q[2]),
+				r[2] * q[0] +  r[0] * q[2] + (r[3] * q[1] - r[1] * q[3]),
+				r[3] * q[0] +  r[0] * q[3] + (r[1] * q[2] - r[2] * q[1])});
+	}
+	
+	public static double [] normSign(double [] q) {
+		if (q[0] >= 0) return q;
+		return new double [] {-q[0], -q[1], -q[2], -q[3]};
+	}
+	
+	
+	/**
+	 * Apply quaternion q to quaternion r
+	 * @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
+	 * @param r - 4 components (scalar, vector) of the target quaternion to which to apply the first one
+	 * @return composed quaternion
+	 */
+	public static double [] composeQR_Q(
+			double [] q,
+			double [] r) {
+		return normSign(new double [] {
+				  -q[0]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
+				  -q[1]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
+				  -q[2]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
+				  -q[3]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]),// s[3];
+				  
+				   q[1]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
+				  -q[0]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
+				  +q[2]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]) // s[3]
+				  -q[3]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]),// s[2]);
+
+				   q[2]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
+				  -q[0]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
+				  +q[3]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
+				  -q[1]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]),// s[3]);
+
+				   q[3]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
+				  -q[0]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]) // s[3]
+				  +q[1]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
+				  -q[2]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]);
+		});	
+	}
+	
+	public static double [] addTo(
+			double [] offs,
+			double [] xyz) {
+		return new double [] {
+				offs[0]+xyz[0],
+				offs[1]+xyz[1],
+				offs[2]+xyz[2]};
+	}
+	
+	
+	
+}

src/main/java/com/elphel/imagej/tileprocessor/Interscene.java

@@ -7947,6 +7947,21 @@ public class Interscene {
     }
     
     // relative to the GPS/compass
+    /**
+     * Finding correspondence between scene XYZ (ers_reference.getSceneXYZ(ts)) and IMS axes (
+     *  Imx5.applyQuaternionTo(
+	 *				Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
+	 *						ims_mount_atr, // small misalignment, provided in clt_parameters  
+	 *						ims_ortho),    // fixed 90-degree rotation
+     * @param clt_parameters - configuration parameters
+     * @param quadCLTs
+     * @param ref_scene
+     * @param earliest_scene
+     * @param rms
+     * @param enu_corr
+     * @param debugLevel
+     * @return
+     */
     public static double [] getQuaternionCorrection(
 			CLTParameters  clt_parameters,
 			QuadCLT []     quadCLTs,

src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java

@@ -85,7 +85,18 @@ public class IntersceneMatchParameters {
 	public  double   ims_local_ts =  1763235471.513914; // local timestamp from the log
 	public  double   ims_master_ts = 1763235483.411892; // corresponding image TS
 	public  double   ims_master_scale = 4.976863288929714e-06; // (dmaster-dlocal)/dmaster
-	
+	// IMS verical by LMA
+	
+	public  boolean  imsv_calc =         false;  // calculate and set imsv_xyz
+	public  double   imsv_ts0 =  1763232236.0;   // Master timestamp to start processing quaternions to average vertical direction
+	public  double   imsv_ts1 =  1763234000.0;   // Master timestamp to finish processing quaternions to average vertical direction
+	public  double   imsv_lambda =          0.1;
+	public  double   imsv_lambda_scale_good=0.5;
+	public  double   imsv_lambda_scale_bad= 8.0;
+	public  double   imsv_lambda_max =    100;
+	public  double   imsv_rms_diff =        0.00001;
+	public  int      imsv_num_iter =       20;
+	public  double []imsv_xyz =            {0,0,1}; 
 	
 	public  boolean sfm_use =            true;   // use SfM to improve depth map
 	public  double  sfm_min_base =       0.6;    // 2.0;    // use SfM if baseline exceeds this
@@ -1401,6 +1412,28 @@ min_str_neib_fpn	0.35
 		gd.addNumericField("Master clock correction",               1e6*this.ims_master_scale, 6,20,"ppm",
 				"Relastive master clock duration (master_period-local_period)/master_period, parts per million.");
 		
+		gd.addMessage  ("Extracting vertical from the IMS data");
+		gd.addCheckbox ("Calculate and set vertical direction (imsv_xyz)",this.imsv_calc,
+				"Calcualte average vertical from IMS quaternions and set it in parameters.");
+		gd.addNumericField("Timestamp to start vertical averaging",       this.imsv_ts0, 6,20,"s",
+				"Discard all data before. Use negative value for no limit.");
+		gd.addNumericField("Timestamp to finish vertical averaging",      this.imsv_ts1, 6,20,"s",
+				"Discard all data after. Use negative value for no limit.");
+		gd.addNumericField("LMA lambda",                                  this.imsv_lambda, 6,8,"",
+				"Initial value of the LMA lambda");
+		gd.addNumericField("Scale lambda after successful LMA iteration", this.imsv_lambda_scale_good, 3,5,"",
+				"Scale lambda (reduce) if the new RMSE is lower than the previous one.");
+		gd.addNumericField("Scale lambda after failed LMA iteration",     this.imsv_lambda_scale_bad, 3,5,"",
+				"Scale lambda (increase) if the new RMSE is higher than the previous one.");
+		gd.addNumericField("Maximal value of lambda to try",              this.imsv_lambda_max, 2,7,"",
+				"Fail LMA if the result is still worse than before parameters were updates.");
+		gd.addNumericField("Minimal relative RMSE improvement",           this.imsv_rms_diff, 8,10,"",
+				"Exit LMA iterations if relative RMSE improvement drops below this value.");
+		gd.addNumericField("Maximal number of LMA iterations",            this.imsv_num_iter, 0,3,"",
+				"A hard limit on LMA iterations.");
+		gd.addStringField ("Average vertical direction", IntersceneMatchParameters.doublesToString(this.imsv_xyz), 80,
+				"Calculated vertical direction.");
+		
 		gd.addTab         ("SfM", "Structure from Motion to improve depth map for the lateral views");
 		gd.addCheckbox ("Use SfM",                                   this.sfm_use,
 				"Use SfM for the depth map enhancement for laterally moving camera.");
@@ -3376,6 +3409,18 @@ min_str_neib_fpn	0.35
 		this.ims_master_ts =                  gd.getNextNumber();
 		this.ims_master_scale =               1e-6*gd.getNextNumber();
 		
+		this.imsv_calc =                      gd.getNextBoolean();
+		this.imsv_ts0 =                       gd.getNextNumber();
+		this.imsv_ts1 =                       gd.getNextNumber();
+		this.imsv_lambda =                    gd.getNextNumber();
+		this.imsv_lambda_scale_good =         gd.getNextNumber();
+		this.imsv_lambda_scale_bad =          gd.getNextNumber(); 
+		this.imsv_lambda_max =                gd.getNextNumber(); 
+		this.imsv_rms_diff =                  gd.getNextNumber();
+		this.imsv_num_iter =            (int) gd.getNextNumber();
+		this.imsv_xyz =  IntersceneMatchParameters.StringToDoubles(gd.getNextString(), imsv_xyz);
+		
+		
 		this.sfm_use =                        gd.getNextBoolean();
 		this.sfm_min_base =                   gd.getNextNumber();
 		this.sfm_min_gain =                   gd.getNextNumber();
@@ -4548,6 +4593,18 @@ min_str_neib_fpn	0.35
 		properties.setProperty(prefix+"ims_local_ts",                  this.ims_local_ts+"");                  // double
 		properties.setProperty(prefix+"ims_master_ts",                 this.ims_master_ts+"");                 // double
 		properties.setProperty(prefix+"ims_master_scale",              this.ims_master_scale+"");              // double
+		
+		properties.setProperty(prefix+"imsv_calc",                     this.imsv_calc + "");                   // boolean
+		properties.setProperty(prefix+"imsv_ts0",                      this.imsv_ts0+"");                      // double
+		properties.setProperty(prefix+"imsv_ts1",                      this.imsv_ts1+"");                      // double
+		properties.setProperty(prefix+"imsv_lambda",                   this.imsv_lambda+"");                   // double
+		properties.setProperty(prefix+"imsv_lambda_scale_good",        this.imsv_lambda_scale_good+"");        // double
+		properties.setProperty(prefix+"imsv_lambda_scale_bad",         this.imsv_lambda_scale_bad+"");         // double
+		properties.setProperty(prefix+"imsv_lambda_max",               this.imsv_lambda_max+"");               // double
+		properties.setProperty(prefix+"imsv_rms_diff",                 this.imsv_rms_diff+"");                 // double
+		properties.setProperty(prefix+"imsv_num_iter",                 this.imsv_num_iter+"");                 // int
+		properties.setProperty(prefix+"imsv_xyz",   IntersceneMatchParameters.doublesToString(this.imsv_xyz));		
+		
 		properties.setProperty(prefix+"sfm_use",                       this.sfm_use + "");                     // boolean
 		properties.setProperty(prefix+"sfm_min_base",                  this.sfm_min_base+"");                  // double
 		properties.setProperty(prefix+"sfm_min_gain",                  this.sfm_min_gain+"");                  // double
@@ -5664,6 +5721,18 @@ min_str_neib_fpn	0.35
 		if (properties.getProperty(prefix+"ims_local_ts")!=null)       this.ims_local_ts=Double.parseDouble(properties.getProperty(prefix+"ims_local_ts"));
 		if (properties.getProperty(prefix+"ims_master_ts")!=null)      this.ims_master_ts=Double.parseDouble(properties.getProperty(prefix+"ims_master_ts"));
 		if (properties.getProperty(prefix+"ims_master_scale")!=null)   this.ims_master_scale=Double.parseDouble(properties.getProperty(prefix+"ims_master_scale"));
+		
+		if (properties.getProperty(prefix+"imsv_calc")!=null)          this.imsv_calc=Boolean.parseBoolean(properties.getProperty(prefix+"imsv_calc"));		
+		if (properties.getProperty(prefix+"imsv_ts0")!=null)           this.imsv_ts0=Double.parseDouble(properties.getProperty(prefix+"imsv_ts0"));
+		if (properties.getProperty(prefix+"imsv_ts1")!=null)           this.imsv_ts1=Double.parseDouble(properties.getProperty(prefix+"imsv_ts1"));
+		if (properties.getProperty(prefix+"imsv_lambda")!=null)        this.imsv_lambda=Double.parseDouble(properties.getProperty(prefix+"imsv_lambda"));
+		if (properties.getProperty(prefix+"imsv_lambda_scale_good")!=null)this.imsv_lambda_scale_good=Double.parseDouble(properties.getProperty(prefix+"imsv_lambda_scale_good"));
+		if (properties.getProperty(prefix+"imsv_lambda_scale_bad")!=null) this.imsv_lambda_scale_bad=Double.parseDouble(properties.getProperty(prefix+"imsv_lambda_scale_bad"));
+		if (properties.getProperty(prefix+"imsv_lambda_max")!=null)    this.imsv_lambda_max=Double.parseDouble(properties.getProperty(prefix+"imsv_lambda_max"));
+		if (properties.getProperty(prefix+"imsv_rms_diff")!=null)      this.imsv_rms_diff=Double.parseDouble(properties.getProperty(prefix+"imsv_rms_diff"));
+		if (properties.getProperty(prefix+"imsv_num_iter")!=null)      this.imsv_num_iter=Integer.parseInt(properties.getProperty(prefix+"imsv_num_iter"));
+		if (properties.getProperty(prefix+"imsv_xyz")!=null)           this.imsv_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"imsv_xyz"),this.imsv_xyz);
+		
 		if (properties.getProperty(prefix+"sfm_use")!=null)            this.sfm_use=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_use"));		
 		if (properties.getProperty(prefix+"sfm_min_base")!=null)       this.sfm_min_base=Double.parseDouble(properties.getProperty(prefix+"sfm_min_base"));
 		if (properties.getProperty(prefix+"sfm_min_gain")!=null)       this.sfm_min_gain=Double.parseDouble(properties.getProperty(prefix+"sfm_min_gain"));
@@ -6812,6 +6881,18 @@ min_str_neib_fpn	0.35
  		imp.ims_local_ts                  = this.ims_local_ts;
  		imp.ims_master_ts                 = this.ims_master_ts;
  		imp.ims_master_scale              = this.ims_master_scale;
+ 		
+ 		imp.imsv_calc                     = this.imsv_calc;
+ 		imp.imsv_ts0                      = this.imsv_ts0;
+ 		imp.imsv_ts1                      = this.imsv_ts1;
+ 		imp.imsv_lambda                   = this.imsv_lambda;
+ 		imp.imsv_lambda_scale_good        = this.imsv_lambda_scale_good;
+ 		imp.imsv_lambda_scale_bad         = this.imsv_lambda_scale_bad;
+ 		imp.imsv_lambda_max               = this.imsv_lambda_max;
+ 		imp.imsv_rms_diff                 = this.imsv_rms_diff;
+ 		imp.imsv_num_iter                 = this.imsv_num_iter;
+		imp.imsv_xyz                      = this.imsv_xyz.clone();
+ 		
  		imp.sfm_use                       = this.sfm_use;
  		imp.sfm_min_base                  = this.sfm_min_base;
  		imp.sfm_min_gain                  = this.sfm_min_gain;

src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java

@@ -1583,7 +1583,7 @@ public class QuadCLTCPU {
     /**
      * Integrate position and orientation of the camera relative to the
      * (inertial) reference frame using IMU-derived local angular and 
-     * linera velocities
+     * linear velocities
      * @param clt_parameters configuration parameters
      * @param quadCLTs       scenes sequence
      * @param ref_index      reference scene index 
@@ -2802,6 +2802,11 @@ public class QuadCLTCPU {
     		}
     	}
     	double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
+		if (debug_level > -3) {
+			System.out.println("##### scene_abs_atr_enu = "+scene_abs_atr_enu[0]+", "+scene_abs_atr_enu[1]+", "+scene_abs_atr_enu[2]+" #####");
+		}
+    	
+    	
     	double [][] dls = getDLS();
     	if ((min_sfm_gain> 0) && (dls[3] != null)) {
     		int num_sfm = 0;

src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java

@@ -2203,32 +2203,6 @@ public class QuaternionLma {
 			};
 		}
 	}
-	public static double [][] composeQR_QdQ_(
-			double [] q,
-			double [] r) {
-		return new double [][] {
-			//  t=s*q' d/dQ0    
-			{-2*r[0]*q[0],
-			 -2*r[1]*q[0] - 2*r[2]*q[3] + 2*r[3]*q[2],
-			 -2*r[2]*q[0] - 2*r[3]*q[1] + 2*r[1]*q[3],
-			 -2*r[3]*q[0] - 2*r[1]*q[2] + 2*r[2]*q[1]},
-			// t=s*q' d/dQ1 
-			{-2*r[0]*q[1],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-			 -2*r[1]*q[2] - 2*r[3]*q[0] + 2*r[2]*q[1],
-			 +2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3]},
-			// t=s*q'd/dQ2
-			{-2*r[0]*q[2],
-			 -2*r[2]*q[1] + 2*r[3]*q[0] + 2*r[1]*q[2],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-			 -2*r[2]*q[3] - 2*r[1]*q[0] + 2*r[3]*q[2]},
-			// t=s*q'd/dQ3
-			{-2*r[0]*q[3],
-			 -2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
-			 -2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
-		};
-	}
 
 	/**
 	 * Get derivatives of the composed quaternion (compose(q,r)) by the
@@ -2242,13 +2216,6 @@ public class QuaternionLma {
 	 */
 	public static double [][] composeDQ( // not used
 			double [] r) {
-		/*
-		return new double [][] {
-			{ r[0], -r[1], -r[2], -r[3]},
-			{ r[1],  r[0], -r[3],  r[2]},
-			{ r[2],  r[3],  r[0], -r[1]},
-			{ r[3],	-r[2],	r[1],  r[0]}};
-			*/
 		return new double [][] {
 			{ r[0], r[1], r[2], r[3]},            
 			{-r[1], r[0], r[3],-r[2]},
@@ -2276,13 +2243,6 @@ public class QuaternionLma {
 			{ q[1],  q[0],  q[3], -q[2]},
 			{ q[2], -q[3],  q[0],  q[1]},
 			{ q[3],  q[2], -q[1],  q[0]}};
-/*		
-		return new double [][] {                
-			{ q[0], q[1], q[2], q[3]}, 
-			{-q[1], q[0],-q[3], q[2]}, 
-			{-q[2], q[3], q[0],-q[1]},
-			{-q[3],-q[2], q[1], q[0]}};
-*/			
 	}
 
 	public static double [] addTo(
@@ -2313,7 +2273,7 @@ public class QuaternionLma {
 	}
 	
 	/**
-	 * Get derivatives of the rotated vector (see applyTo(q,xyz)) by the components of the quaterion q
+	 * Get derivatives of the rotated vector (see applyTo(q,xyz)) by the components of the quaternion q
 	 * @param q   4 components (scalar, vector) of the quaternion being applied
 	 *            to as vector.
 	 * @param  xyz 1-d array representing a 3D vector {X, Y, Z}