Debugging OrientationSceneLMA - working, will clean up

src/main/java/com/elphel/imagej/orthomosaic/OrientationSceneLMA.java

@@ -47,7 +47,10 @@ public class OrientationSceneLMA {
 	private double [][]       last_jt =         null;
 	
 	public boolean            last3only =       false;  // true; // debug feature
-	public double             delta=            1e-7;
+	public double             delta=            1e-5; // 7;
+	public boolean            debug_deriv = true;
+	public int                debug_width =     12;
+	public int                debug_decimals =  9;
 
 	
 	public int prepareLMA(
@@ -131,15 +134,38 @@ public class OrientationSceneLMA {
 					double [] qscene1 = new double[4];
 					double [] qscene2 = new double[4];
 					double [][][] derivs = (jt!=null) ? (new double [2][][]): null;
+					double [][][] qderivs = (jt != null) ? (new double [2][][]) : null;
+					
 					for (int npair = ai.getAndIncrement(); npair < num_pairs; npair = ai.getAndIncrement()) {
 						int [] scene_ind = {4 * cpairs[npair][0], 4 * cpairs[npair][1]};
 						System.arraycopy(vector, scene_ind[0], qscene1, 0, 4);
 						System.arraycopy(vector, scene_ind[1], qscene2, 0, 4);
-						double [] fx_frag =getPairScaleDirError( //{scale-1, qn1,qn2,qn3}
+						double [] q2iq1q12;
+						if (debug_deriv && (debug_level>10)) {
+							q2iq1q12 = getPairErrQuaternion(
 									qscene1, // double [] qscene1,
 									qscene2, // double [] qscene2,
 									qpairs[npair],  // double [] qpair12,
-								derivs); // double [][][] derivs) 
+									qderivs,        // double [][][] qderivs)
+									delta,          // double    delta,
+									debug_width,    // int       fmt_width,
+									debug_decimals, // int       fmt_decimals,
+									npair);         // int       npair); 
+
+
+						} else {
+							q2iq1q12 = getPairErrQuaternion( // qderivs here tested
+									qscene1, // double [] qscene1,
+									qscene2, // double [] qscene2,
+									qpairs[npair], // double [] qpair12,
+									qderivs); // double [][][] qderivs)
+						}
+						
+						
+						double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
+						double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
+						double [] fx_frag = {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};
+
 						System.arraycopy(
 								fx_frag,
 								0,
@@ -147,14 +173,46 @@ public class OrientationSceneLMA {
 								fx_frag.length*npair,
 								fx_frag.length);
 						if (jt!=null) {
+							double [][][] qderivs_delta = getPairErrQuaternionDelta(  // test only
+									qscene1,       // double [] q1,
+									qscene2,       // double [] q2,
+									qpairs[npair],
+									delta);        // double delta)
+//							printDerivDelta("npair="+npair+" qderivs[0], delta="+delta, qderivs[0], qderivs_delta[0],debug_width,debug_decimals);
+//							printDerivDelta("npair="+npair+" qderivs[1], delta="+delta, qderivs[1], qderivs_delta[1],debug_width,debug_decimals);
+ 
+							
+							double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
+							double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
+							if (debug_deriv) {
+								double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
+								printDerivDelta("npair="+npair+" dq2iq1q12_norm, delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
+								double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
+								printDerivDelta("npair="+npair+" dscale_dq, delta="+delta, dscale, dscale_delta,debug_width,debug_decimals);
+							}
+							dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
+							if (debug_deriv) {
+								double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
+								double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
+								dq2iq1q12_norm_delta[0] = dscale_delta; // replace first row (q_norm/dq) with dsca/e/dq
+								printDerivDelta("npair="+npair+" dq2iq1q12_normD,delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
+							}							
+							derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
+							derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq2 (scene 2)
+							
 							for (int iscene = 0; iscene <2; iscene++) {
 								for (int npar = 0; npar <4; npar++) {
+									for (int i = 0; i < 4; i++) {
+										jt[scene_ind[iscene]+npar][fx_frag.length*npair+i] = derivs[iscene][i][npar];
+									}
+									/*
 									System.arraycopy(
 											derivs[iscene][npar], // null
 											0,
 											jt[scene_ind[iscene]+npar],
 											fx_frag.length*npair,
 											fx_frag.length);
+									*/
 								}
 							}
 						}
@@ -268,8 +326,9 @@ public class OrientationSceneLMA {
 				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");
+		System.out.print(String.format("%3s","err"));
+		for (int i = 0; i < 2* vector.length; i++) {
+			System.out.print(String.format("\t%12s",""));
 		}			
 		for (int i = 0; i < vector.length; i++) {
 			System.out.print(String.format("\t%12.9f",errors[i]));
@@ -647,7 +706,7 @@ public class OrientationSceneLMA {
 	 * @return
 	 */
 	
-	public static double [] getPairErrQuaternion(
+	public static double [] getPairErrQuaternion0(
 			double [] qscene1,
 			double [] qscene2,
 			double [] qpair12,
@@ -668,11 +727,240 @@ public class OrientationSceneLMA {
 		return q2iq1q12;
 	}
 	
+	public static double [] getPairErrQuaternion(
+			double [] q1,
+			double [] q2,
+			double [] q12,
+			double [][][] qderivs) {
+		double [] iq2 =      QuatUtils.invert(q2);
+		double [] iq2q1 =    QuatUtils.multiply(iq2, q1);
+		double [] iq2q1q12 = QuatUtils.multiply(iq2q1, q12);
+		if (qderivs != null) {
+			double [][] diq2_dq2 =     QuatUtils.d_invert_dq(q2);
+			
+			double [][] diq2q1_dq2 =    QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
+			double [][] diq2q1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq2);
+			
+			double [][] diq2q1_dq1 =    QuatUtils.d_pq_dq(iq2);
+			double [][] diq2q1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq1);
+			qderivs[0] = diq2q1q12_dq1;
+			qderivs[1] = diq2q1q12_dq2;
+		}
+		return iq2q1q12;
+	}
+	
+	public static double [] getPairErrQuaternion( // test only
+			double [] q1,
+			double [] q2,
+			double [][][] qderivs) {
+		double [] iq2 =      QuatUtils.invert(q2);
+		double [] iq2q1 =    QuatUtils.multiply(iq2, q1);
+		if (qderivs != null) {
+			double [][] diq2_dq2 =     QuatUtils.d_invert_dq(q2);
+			double [][] diq2q1_dq2 =    QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
+			double [][] diq2q1_dq1 =    QuatUtils.d_pq_dq(iq2);
+			qderivs[0] = diq2q1_dq1;
+			qderivs[1] = diq2q1_dq2;
+		}
+		return iq2q1;
+	}
+	
+	public static double [][][] getPairErrQuaternionDelta(  // test only
+			double [] q1,
+			double [] q2,
+			double delta) {
+		double [][][] qderivs =  new double [2][4][4];
+		for (int nscene = 0; nscene <2; nscene++) {
+			for (int npar = 0; npar < 4; npar++) {
+				double [][] vpm = new double [][] {q1.clone(),q2.clone()};
+				vpm[nscene][npar] += 0.5*delta;
+				double [] qd_p = getPairErrQuaternion(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						null);  // double [][][] qderivs)
+				vpm[nscene][npar]-= delta;
+				double [] qd_m = getPairErrQuaternion(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						null);  // double [][][] qderivs)
+				for (int i = 0; i < 4; i++) {
+					qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta; 
+				}
+//				System.out.println("nscene="+nscene+", npar="+npar);
+			}
+		}
+		return qderivs;
+	}
+	
+	
+
+	public static void testGetPairErrQuaternion () {
+		double delta = 1e-5;
+		int debug_width = 12,debug_decimals=9;
+		double [] q1 = {0.9995, 0.01, 0.015, 0.02}; 
+		double [] q2 = {0.999,  0.02, 0.01, 0.015};
+		double [] q12 = {1.1,   0.1, 0.15, -0.2};
+		double [][][] qderivs =  new double [2][4][4];
+		for (int n = 0; n < 10; n++) {
+			 getPairErrQuaternion( // test only
+						q1, // double [] q1,
+						q2, // double [] q2,
+						q12,
+						qderivs, // double [][][] qderivs)
+						delta, // double    delta,
+						debug_width, // int       fmt_width,
+						debug_decimals, // int       fmt_decimals,
+						0); // int       npair) { // 
+			double [][][] qderivs_delta = getPairErrQuaternionDelta(  // test only
+					q1, // double [] q1,
+					q2, // double [] q2,
+					q12,
+					delta); // double delta)
+			
+			printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
+			printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
+			System.out.println();
+		}
+	}
+	
+	
+	public static void testGetPairPairScaleDirError () {
+		double delta = 1e-5;
+		int debug_width = 12,debug_decimals=9;
+		double [] q1 = {0.9995, 0.01, 0.015, 0.02}; 
+		double [] q2 = {0.999,  0.02, 0.01, 0.015};
+		double [] q12 = {1.1,   0.1, 0.15, -0.2};
+		double [][][] qderivs =  new double [2][4][4];
+		for (int n = 0; n < 10; n++) {
+			/*
+			 getPairErrQuaternion( // test only
+						q1, // double [] q1,
+						q2, // double [] q2,
+						q12,
+						qderivs); // double [][][] qderivs)
+						*/
+			/*
+			 getPairErrQuaternion( // test only
+						q1, // double [] q1,
+						q2, // double [] q2,
+						q12,
+						qderivs, // double [][][] qderivs)
+						delta, // double    delta,
+						debug_width, // int       fmt_width,
+						debug_decimals, // int       fmt_decimals,
+						0); // int       npair) { // 
+			 */
+			getPairScaleDirError(
+					q1, // double [] q1,
+					q2, // double [] q2,
+					q12,
+					qderivs); // double [][][] qderivs)
+/*			 
+			double [][][] qderivs_delta = getPairErrQuaternionDelta(  // test only
+					q1, // double [] q1,
+					q2, // double [] q2,
+					q12,
+					delta); // double delta)
+*/					
+			double [][][] qderivs_delta = getPairScaleDirError(  // test only
+					q1, // double [] q1,
+					q2, // double [] q2,
+					q12,
+					delta); // double delta)
+			
+			printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
+			printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
+			System.out.println();
+		}
+	}
+	
+	
+	
+	
+	
+	
+	public static double [] getPairErrQuaternion(
+			double [] qscene1,
+			double [] qscene2,
+			double [] qpair12,
+			double [][][] qderivs,
+			double    delta,
+			int       fmt_width,
+			int       fmt_decimals,
+			int       npair) { // 
+		double [] iqscene2 = QuatUtils.invert(qscene2);
+		double [] iq2q1 =    QuatUtils.multiply(iqscene2, qscene1);
+		double [] q2iq1q12 = QuatUtils.multiply(iq2q1, qpair12);
+		if (qderivs != null) {
+			double [][] diqscene2_dq2 = QuatUtils.d_invert_dq(qscene2);
+			{
+				double [][] diqscene2_dq2_delta=QuatUtils.d_invert_dq(qscene2,delta);
+				printDerivDelta("npair="+npair+" diqscene2_dq2, delta="+delta, diqscene2_dq2, diqscene2_dq2_delta,fmt_width,fmt_decimals);
+			}			
+			double [][] diq2q1_dq2 =    QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1), diqscene2_dq2);
+			{
+				double [][] diq2q1_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1, delta), diqscene2_dq2);
+				printDerivDelta("npair="+npair+" diq2q1_dq2, delta="+delta, diq2q1_dq2, diq2q1_dq2_delta,fmt_width,fmt_decimals);
+			}			
+			double [][] dq2iq1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq2);
+			{
+				double [][] dq2iq1q12_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq2);
+				printDerivDelta("npair="+npair+" dq2iq1q12_dq2, delta="+delta, dq2iq1q12_dq2, dq2iq1q12_dq2_delta,fmt_width,fmt_decimals);
+			}			
+			double [][] diq2q1_dq1 =    QuatUtils.d_pq_dq(iqscene2,qscene1);
+			{
+				double [][] diq2q1_dq1_delta= QuatUtils.d_pq_dq(iqscene2,qscene1,delta);
+				printDerivDelta("npair="+npair+" diq2q1_dq1, delta="+delta, diq2q1_dq1, diq2q1_dq1_delta,fmt_width,fmt_decimals);
+			}			
+			double [][] dq2iq1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq1);
+			{
+				double [][] dq2iq1q12_dq1_dq1_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq1);
+				printDerivDelta("npair="+npair+" dq2iq1q12_dq1, delta="+delta, dq2iq1q12_dq1, dq2iq1q12_dq1_dq1_delta,fmt_width,fmt_decimals);
+			}			
+			qderivs[0] = dq2iq1q12_dq1;
+			qderivs[1] = dq2iq1q12_dq2;
+		}
+		return q2iq1q12;
+	}
+
+	
+	public static double [][][] getPairErrQuaternionDelta(
+			double [] qscene1,
+			double [] qscene2,
+			double [] qpair12,
+			double delta) {
+		double [][][] qderivs =  new double [2][4][4];
+		for (int nscene = 0; nscene <2; nscene++) {
+			for (int npar = 0; npar < 4; npar++) {
+				double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
+				vpm[nscene][npar] += 0.5*delta;
+				double [] qd_p = getPairErrQuaternion(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						qpair12,// double [][][] qderivs),
+						null);  // double [][][] qderivs)
+				vpm[nscene][npar]-= delta;
+				double [] qd_m = getPairErrQuaternion(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						qpair12,// double [][][] qderivs),
+						null);  // double [][][] qderivs)
+				for (int i = 0; i < 4; i++) {
+					qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta; 
+				}
+			}
+		}
+		return qderivs;
+	}
+	
+	
+	
 	public static double [] getPairScaleDirError(
 			double [] qscene1,
 			double [] qscene2,
 			double [] qpair12,
 			double [][][] derivs) {
+		boolean truncate = false; // true;
 		double [][][] qderivs = (derivs != null) ? (new double [2][][]) : null;
 		double [] q2iq1q12 = getPairErrQuaternion(
 				qscene1, // double [] qscene1,
@@ -682,15 +970,89 @@ public class OrientationSceneLMA {
 		double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
 		double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
 		if (derivs != null) {
-			double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12_norm);
-			double [] dscale = QuatUtils.dscale_dq(q2iq1q12_norm);
+			if (truncate) {
+				derivs[0] = qderivs[0];
+				derivs[1] = qderivs[1];
+				
+			} else {
+				double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
+				double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
 				dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
 				derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
 				derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq1 (scene 2)
 			}
-		
+		}		
+		if (truncate) return q2iq1q12;
 		return new double [] {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};
 	}
 
 	
+	
+	
+	public static double [][][] getPairScaleDirError(
+			double [] qscene1,
+			double [] qscene2,
+			double [] qpair12,
+			double delta) {
+		double [][][] sd_derivs =  new double [2][4][4];
+		for (int nscene = 0; nscene <2; nscene++) {
+			for (int npar = 0; npar < 4; npar++) {
+				double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
+				vpm[nscene][npar] += 0.5*delta;
+				double [] qd_p = getPairScaleDirError(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						qpair12,// double [][][] qderivs),
+						null);  // double [][][] qderivs)
+				vpm[nscene][npar]-= delta;
+				double [] qd_m = getPairScaleDirError(
+						vpm[0], // double [] qscene1,
+						vpm[1], // double [] qscene2,
+						qpair12,// double [][][] qderivs),
+						null);  // double [][][] qderivs)
+				for (int i = 0; i < 4; i++) {
+					sd_derivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta; 
+				}
+			}
+		}
+		return sd_derivs;
+	}
+	
+	
+	
+	
+	public static void printDerivDelta(String name, double [] deriv, double [] delta, int width, int decimals) {
+		double max_err = 0;
+		double [] diff= new double [deriv.length];
+		String fmt ="\t%"+width+"."+decimals+"f"; 
+		for (int i = 0; i < diff.length; i++) {
+			diff[i] = deriv[i]-delta[i];
+			max_err = Math.max(max_err, Math.abs(diff[i]));
+		}
+		System.out.println (name+", max_err = "+max_err);
+		System.out.print ("deriv ");
+		for (int i = 0; i < diff.length; i++) 	System.out.print(String.format(fmt, deriv[i]));
+		System.out.println();
+		System.out.print ("delta ");
+		for (int i = 0; i < diff.length; i++) 	System.out.print(String.format(fmt, delta[i]));
+		System.out.println();
+		System.out.print (" diff ");
+		for (int i = 0; i < diff.length; i++) 	System.out.print(String.format(fmt, diff[i]));
+		System.out.println();
+	}
+
+	public static void printDerivDelta(String name, double [][] deriv2, double [][] delta2, int width, int decimals) {
+		int l = deriv2[0].length;
+		double [] deriv = new double [deriv2.length*l];
+		double [] delta = new double [deriv.length];
+		for (int i = 0; i < deriv2.length; i++) {
+			for (int j = 0; j <l; j++) {
+				deriv[i*l+j] = deriv2[i][j];
+				delta[i*l+j] = delta2[i][j];
+			}
+		}
+		printDerivDelta(name, deriv, delta, width, decimals);
+	}
+	
+	
 }

src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java

@@ -78,6 +78,9 @@ public class OrthoAltitudeMatch {
         
         boolean do_not_save = true;
 
+//        OrientationSceneLMA.testGetPairErrQuaternion ();
+        OrientationSceneLMA.testGetPairPairScaleDirError();
+        
 		if (test_quat0) {
 			QuatUtils.testQuatAff();
 		}

src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java

@@ -6286,6 +6286,11 @@ public class OrthoMapsCollection  implements Serializable{
 	public boolean altutudeMatchPairs(
 			CLTParameters clt_parameters,
 			String        orthoMapsCollection_path) {
+        //OrientationSceneLMA.testGetPairErrQuaternion ();	
+
+
+        
+        
         // Create list of all pairs (after recreating all overlaps with updated affines)
 		ArrayList<Point> pairs_list = new ArrayList<Point>();
 		boolean dbg00 = false;

src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java

@@ -70,6 +70,7 @@ public final class QuatUtils { // static class
 	/**
 	 * Differential d_(p*q)/dp
 	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
+	 * Seems that (11) for []x is transposed!
 	 * @param p quaternion
 	 * @param q quaternion
 	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,
@@ -79,16 +80,57 @@ public final class QuatUtils { // static class
 			double [] p,
 			double [] q) {
 		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]}  
+/*
 			{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 [][] d_pq_dp(
+			double [] q) {
+		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 [][] d_pq_dp(
+			double [] p,
+			double [] q,
+			double delta) {
+		double [][] d_pq_dp_delta=new double[4][4];
+		for (int npar = 0; npar < 4; npar++) {
+			double [] vpm = p.clone();
+			vpm[npar] += 0.5*delta;
+			double [] qd_p = multiply(vpm, q);
+			vpm[npar] -= delta;
+			double [] qd_m = multiply(vpm, q);
+			for (int i = 0; i < 4; i++) {
+				d_pq_dp_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
+			}
+		}
+		return d_pq_dp_delta;
+	}
+	
+	
+	
 	/**
 	 * Differential d_(p*q)/dq
 	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
+	 * Seems that (11) for []x is transposed!
 	 * @param p quaternion
 	 * @param q quaternion
 	 * @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,
@@ -98,13 +140,51 @@ public final class QuatUtils { // static class
 			double [] p,
 			double [] q) {
 		return new double[][] {
+			{p[0],-p[1],-p[2], -p[3]},
+			{p[1], p[0], p[3], -p[2]},
+			{p[2],-p[3], p[0],  p[1]},
+			{p[3], p[2],-p[1],  p[0]}
+/*
 			{p[0],-p[1],-p[2], -p[3]},
 			{p[1], p[0],-p[3],  p[2]},
 			{p[2], p[3], p[0], -p[1]},
 			{p[3],-p[2], p[1],  p[0]}  
+
+ */
 		};
 	}
 
+	public static double [][] d_pq_dq(
+			double [] p) {
+		return new double[][] {
+			{p[0],-p[1],-p[2], -p[3]},
+			{p[1], p[0], p[3], -p[2]},
+			{p[2],-p[3], p[0],  p[1]},
+			{p[3], p[2],-p[1],  p[0]}		};
+	}
+
+	
+	
+	public static double [][] d_pq_dq(
+			double [] p,
+			double [] q,
+			double delta) {
+		double [][] d_pq_dq_delta=new double[4][4];
+		for (int npar = 0; npar < 4; npar++) {
+			double [] vpm = q.clone();
+			vpm[npar] += 0.5*delta;
+			double [] qd_p = multiply(p, vpm);
+			vpm[npar] -= delta;
+			double [] qd_m = multiply(p, vpm);
+			for (int i = 0; i < 4; i++) {
+				d_pq_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
+			}
+		}
+		return d_pq_dq_delta;
+	}
+	
+	
+	
 	/**
 	 * Derivative of inverse quanternion
 	 * @param q quternion
@@ -123,6 +203,25 @@ public final class QuatUtils { // static class
 		};
 	}
 	
+	public static double [][] d_invert_dq(
+			double [] q,
+			double delta) {
+		double [][] d_invert_dq_delta=new double[4][4];
+		for (int npar = 0; npar < 4; npar++) {
+			double [] vpm = q.clone();
+			vpm[npar] +=  0.5*delta;
+			double [] qd_p = invert(vpm);
+			vpm[npar] -= delta;
+			double [] qd_m = invert(vpm);
+			for (int i = 0; i < 4; i++) {
+				d_invert_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
+			}
+		}
+		return d_invert_dq_delta;
+	}
+
+	
+	
 	public static double [][] dnormalize_dq(
 			double [] q){
 		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
@@ -135,6 +234,26 @@ public final class QuatUtils { // static class
 			{     -q3*q0 /l3,      -q3*q1 /l3,      -q3*q2 /l3, (l2 - q3*q3)/l3}
 		};		
 	}
+	
+	public static double [][] dnormalize_dq(
+			double [] q,
+			double delta) {
+		double [][] dnormalize_dq_delta=new double[4][4];
+		for (int npar = 0; npar < 4; npar++) {
+			double [] vpm = q.clone();
+			vpm[npar] +=  0.5*delta;
+			double [] qd_p = normalize(vpm);
+			vpm[npar] -= delta;
+			double [] qd_m = normalize(vpm);
+			for (int i = 0; i < 4; i++) {
+				dnormalize_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
+			}
+		}
+		return dnormalize_dq_delta;
+	}
+
+	
+	
 	public static double [] dscale_dq(
 			double [] q){
 		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
@@ -142,6 +261,21 @@ public final class QuatUtils { // static class
 		return new double [] {q0/l, q1/l, q2/l,q3/l}; 
 	}
 	
+	public static double [] dscale_dq(
+			double [] q,
+			double delta) {
+		double [] dscale_dq_delta=new double[4];
+		for (int npar = 0; npar < 4; npar++) {
+			double [] vpm = q.clone();
+			vpm[npar] +=  0.5*delta;
+			double  qd_p = norm(vpm);
+			vpm[npar] -= delta;
+			double  qd_m = norm(vpm);
+			dscale_dq_delta[npar] = (qd_p-qd_m)/delta;
+		}
+		return dscale_dq_delta;
+	}
+	
 	
 	
 	/**