Commit 8adc63c0 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

fixed align heading crossing south

parent 113b0bf8
...@@ -236,17 +236,35 @@ function hll2_w_i(i,v){ ...@@ -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) * Functions for position latitude and longitude (heading is fixed)
* hll3_... * hll3_...
*/ */
function hll3_r_i(i,v){ function hll3_r_i(i,v){
var heading = Data.camera.kml.heading; var heading = Data.camera.kml.heading;
var f1 = hll_f_3d_i(i,[v[0],v[1],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]); 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+360)%360;
return (f1-f2); return (f1-f2);
} }
......
...@@ -102,6 +102,154 @@ numbers.calculus.GaussNewton = function(v,n,r,dr,eps,w){ ...@@ -102,6 +102,154 @@ numbers.calculus.GaussNewton = function(v,n,r,dr,eps,w){
J = numbers.matrix.multiply(J,Jt) J = numbers.matrix.multiply(J,Jt)
var V = [] 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++){ for(var i=0;i<n;i++){
V.push([wn(i,v,wsum)*r(i,v)]) V.push([wn(i,v,wsum)*r(i,v)])
} }
......
...@@ -401,6 +401,7 @@ function x3dom_align_hll3(){ ...@@ -401,6 +401,7 @@ function x3dom_align_hll3(){
var xyh = [x0,y0]; 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(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]; xyh = [result.v[0],result.v[1],h0];
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment