fixed align heading crossing south

js/align_functions.js

@@ -236,17 +236,35 @@ function hll2_w_i(i,v){
 
 }
 
+function bring_degrees_to_n180_180(a){
+
+  while(a<-180){
+    a += 360;
+  }
+  while(a>180){
+    a -= 360;
+  }
+  return a;
+
+}
+
 /**
  * Functions for position latitude and longitude (heading is fixed)
  * hll3_...
  */
-
 function hll3_r_i(i,v){
 
   var heading = Data.camera.kml.heading;
 
   var f1 = hll_f_3d_i(i,[v[0],v[1],heading]);
   var f2 = hll_f_map_i(i,[v[0],v[1],heading]);
+
+  f1 -= heading;
+  f2 -= heading;
+
+  f1 = bring_degrees_to_n180_180(f1);
+  f2 = bring_degrees_to_n180_180(f2);
+
   //return (f1-f2+360)%360;
   return (f1-f2);
 }

js/numbers/numbers.calculus.extra.js

@@ -102,6 +102,154 @@ numbers.calculus.GaussNewton = function(v,n,r,dr,eps,w){
     J = numbers.matrix.multiply(J,Jt)
 
     var V = []
+
+    for(var i=0;i<n;i++){
+      V.push([wn(i,v,wsum)*r(i,v)])
+    }
+
+    var delta = numbers.matrix.multiply(J,V)
+
+    //console.log("delta: ");
+    //console.log(delta);
+
+    var res = []
+
+    for(var i=0;i<v.length;i++){
+      res[i] = v[i]-delta[i][0]
+    }
+
+    return res
+
+  }
+
+  function sigma(v,n,r){
+
+    var sum = 0
+    var wsum = ws(v,n)
+
+    for(var i=0;i<n;i++){
+      sum += wn(i,v,wsum)*r(i,v)*r(i,v)
+      //wsum += w(i,v)
+    }
+
+    //console.log("sum = "+sum+" wsum = "+wsum);
+
+    //sum = Math.sqrt(sum/wsum)
+    sum = Math.sqrt(sum)
+
+    return sum
+
+  }
+
+  function jacobian(v,n,dr){
+
+    var J = []
+
+    for(var i=0;i<n;i++){
+
+      var row = []
+      for(var j=0;j<dr.length;j++){
+        row.push(dr[j](i,v))
+      }
+      J[i] = row
+
+    }
+
+    return J
+
+  }
+
+  // normalized weight
+  function wn(i,v,wsum){
+
+    return w(i,v)/wsum
+
+  }
+
+  // sum of weights for normalization
+  function ws(v,n){
+
+    var wsum = 0
+
+    for(var i=0;i<n;i++){
+      wsum += w(i,v)
+    }
+
+    return wsum
+
+  }
+
+}
+
+// v   - is a vector of parameters
+// n   - number of measurements
+// r   - residual function
+// dr  - partial derivatives
+// eps - epsilon
+// w   - weights
+
+numbers.calculus.GaussNewton_nD = function(v,n,r,dr,eps,w){
+
+  var epsilon = eps || 1e-8
+  var limit = 1000
+
+  var stop = false
+  var counter = 0
+  var v0 = v
+
+  if (w===undefined){
+    w = function(){
+      return 1;
+    };
+  }
+
+  while(!stop){
+
+    counter++
+
+    var v1 = iterate(v0,n,r,dr)
+
+    //console.log(v1);
+
+    var s0 = sigma(v0,n,r)
+    var s1 = sigma(v1,n,r)
+
+    if((Math.abs(s1-s0)<epsilon)||(counter==limit)){
+      stop = true
+    }
+
+    v0 = v1
+
+  }
+
+  return {
+    count: counter,
+    error: s1,
+    v: v0
+  }
+
+  //functions
+  function iterate(v,n,r,dr){
+
+    var wsum = ws(v,n)
+
+    var J = jacobian(v,n,dr)
+    var Jt = numbers.matrix.transpose(J)
+
+    for(var i=0;i<n;i++){
+      J = numbers.matrix.rowScale(J,i,wn(i,v,wsum))
+    }
+
+    // JtJ
+    J = numbers.matrix.multiply(Jt,J)
+
+    // (Jt x J)^-1
+    J = numbers.matrix.inverse(J)
+    // (Jt x J)^-1 x Jt
+    J = numbers.matrix.multiply(J,Jt)
+
+    var V = []
+
     for(var i=0;i<n;i++){
       V.push([wn(i,v,wsum)*r(i,v)])
     }

js/ui_align.js

@@ -401,6 +401,7 @@ function x3dom_align_hll3(){
   var xyh = [x0,y0];
 
   var result = numbers.calculus.GaussNewton(xyh,Data.markers.length,hll3_r_i,[hll3_dr_dx_i,hll3_dr_dy_i],epsilon,hll3_w_i);
+  //var result = numbers.calculus.GaussNewton_nD(xyh,Data.markers.length,hll3_r_i,[hll3_dr_dx_i,hll3_dr_dy_i],epsilon,hll3_w_i);
 
   xyh = [result.v[0],result.v[1],h0];