diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml
index 30f7f14788..f4d8ec8237 100644
--- a/sw/lib/ocaml/latlong.ml
+++ b/sw/lib/ocaml/latlong.ml
@@ -553,38 +553,6 @@ let geocentric_of_ecef = fun r ->
   (phiP, lambda)
 
 
-(* http://en.wikipedia.org/wiki/Geodetic_system
-   Approximation using the geocentric latitude instead of the geodetic one *)
-let ned_of_ecef = fun r ->
-  let phiP, lambda = geocentric_of_ecef r in
-  let sin_lambda = sin lambda
-  and cos_lambda = cos lambda
-  and sin_phiP = sin phiP
-  and cos_phiP = cos phiP in
-
-  fun p ->
-    let x_xr = p.(0) -. r.(0)
-    and y_yr = p.(1) -. r.(1) 
-    and z_zr = p.(2) -. r.(2) in 
-    
-    let e = -.sin_lambda*.x_xr +. cos_lambda*.y_yr
-    and n = -.sin_phiP*.cos_lambda*.x_xr -. sin_phiP*.sin_lambda*.y_yr +. cos_phiP*.z_zr
-    and u = cos_phiP*.cos_lambda*.x_xr +. cos_phiP*.sin_lambda*.y_yr +. sin_phiP*.z_zr in
-    [|n; e; -.u|]
-      
-let ecef_of_ned = fun r ->
-  let phiP, lambda = geocentric_of_ecef r in
-  let sin_lambda = sin lambda
-  and cos_lambda = cos lambda
-  and sin_phiP = sin phiP
-  and cos_phiP = cos phiP in
-  fun ned ->
-    let n = ned.(0) and e = ned.(1) and u = -.ned.(2) in
-    let x = -.sin_lambda*.e -. cos_lambda*.sin_phiP*.n +. cos_lambda*.cos_phiP*.u +. r.(0)
-    and y =  cos_lambda*.e -. sin_lambda*.sin_phiP*.n +. cos_phiP*.sin_lambda*.u +. r.(1)
-    and z = cos_phiP*.n +. sin_phiP*.u +. r.(2) in
-    [|x; y; z|]
-
 let ecef_of_geo = fun geo ->
   let elps = ellipsoid_of geo in
   let e2 = 2.*.elps.df -. elps.df*.elps.df in
@@ -630,5 +598,35 @@ let geo_of_ecef = fun geo ->
     
     ({posn_lat = phi; posn_long = lambda}, h)
 
+(* http://en.wikipedia.org/wiki/Geodetic_system
+   Approximation using the geocentric latitude instead of the geodetic one *)
+let ned_of_ecef = fun r ->
+  let wgs84, _h = geo_of_ecef WGS84 r in
+  let sin_lambda = sin wgs84.posn_long
+  and cos_lambda = cos wgs84.posn_long
+  and sin_phiP = sin wgs84.posn_lat
+  and cos_phiP = cos wgs84.posn_lat in
+
+  fun p ->
+    let x_xr = p.(0) -. r.(0)
+    and y_yr = p.(1) -. r.(1) 
+    and z_zr = p.(2) -. r.(2) in 
     
+    let e = -.sin_lambda*.x_xr +. cos_lambda*.y_yr
+    and n = -.sin_phiP*.cos_lambda*.x_xr -. sin_phiP*.sin_lambda*.y_yr +. cos_phiP*.z_zr
+    and u = cos_phiP*.cos_lambda*.x_xr +. cos_phiP*.sin_lambda*.y_yr +. sin_phiP*.z_zr in
+    [|n; e; -.u|]
       
+let ecef_of_ned = fun r ->
+  let wgs84, _h = geo_of_ecef WGS84 r in
+  let sin_lambda = sin wgs84.posn_long
+  and cos_lambda = cos wgs84.posn_long
+  and sin_phiP = sin wgs84.posn_lat
+  and cos_phiP = cos wgs84.posn_lat in
+  fun ned ->
+    let n = ned.(0) and e = ned.(1) and u = -.ned.(2) in
+    let x = -.sin_lambda*.e -. cos_lambda*.sin_phiP*.n +. cos_lambda*.cos_phiP*.u +. r.(0)
+    and y =  cos_lambda*.e -. sin_lambda*.sin_phiP*.n +. cos_phiP*.sin_lambda*.u +. r.(1)
+    and z = cos_phiP*.n +. sin_phiP*.u +. r.(2) in
+    [|x; y; z|]
+
diff --git a/sw/lib/ocaml/test/test_latlong.ml b/sw/lib/ocaml/test/test_latlong.ml
index cdc44f5342..cce6f8fc5d 100644
--- a/sw/lib/ocaml/test/test_latlong.ml
+++ b/sw/lib/ocaml/test/test_latlong.ml
@@ -20,7 +20,7 @@ let llh_eq = fun (ll, h) (ll', h') ->
 
 let () = 
   let muret_geo = make_geo_deg 43.46223 1.27289
-  and muret_h = 42. in
+  and muret_h = 185. in
 
   printf "Muret LLH: %s %.2f\n%!" (string_degrees_of_geographic muret_geo) muret_h;
 
@@ -28,7 +28,7 @@ let () =
   printf "Muret ECEF: %a\n%!" fprint_ecef muret_ecef;
 
   (* http://www.apsalin.com/convert-geodetic-to-cartesian.aspx *)
-  assert (ecef_eq muret_ecef (make_ecef [|4635666.15832458; 103003.469232535; 4364945.7959132|]));
+  assert (ecef_eq muret_ecef (make_ecef [|4635769.92611344; 103005.774929684; 4365044.16221717|]));
 
   let muret_geo', muret_h' = geo_of_ecef WGS84 muret_ecef in
   printf "Muret LLH': %s %.2f\n%!" (string_degrees_of_geographic muret_geo')muret_h';
@@ -38,28 +38,39 @@ let () =
   let ecef_of_ned_muret = ecef_of_ned muret_ecef
   and ned_of_ecef_muret = ned_of_ecef muret_ecef in
 
-  let plane_ned = make_ned [| 155.; 255.; -65.|] in
-  
-  printf "Plane NED: %a\n%!" fprint_ned plane_ned;
-
-  let plane_ecef = ecef_of_ned_muret plane_ned in
-
-  printf "Plane ECEF: %a\n%!" fprint_ecef plane_ecef;
-  
-  let plane_ned' = ned_of_ecef_muret plane_ecef in
-
-  printf "Plane NED': %a\n%!" fprint_ned plane_ned';
-  assert (ned_eq plane_ned plane_ned');
-
-  (* Angles in UTM and NED *)
-  let plane_geo, plane_h = geo_of_ecef WGS84 plane_ecef in
-  printf "Plane LLH': %s %.2f\n%!" (string_degrees_of_geographic plane_geo) plane_h;
-
-  let plane_utm = utm_of WGS84 plane_geo
-  and muret_utm = utm_of WGS84 muret_geo in
-
+  let muret_utm = utm_of WGS84 muret_geo in
   printf "Muret UTM: %a\n%!" fprint_utm muret_utm;
-  printf "Plane UTM: %a\n%!" fprint_utm plane_utm;
-  printf "Plane/Muret UTM [%.2f %.2f]\n%!" (plane_utm.utm_x -. muret_utm.utm_x) (plane_utm.utm_y -. muret_utm.utm_y)
-  
 
+  for d = 0 to 5 do
+    let d = 10.**float d in
+    printf "d=%.0fm\n" d;
+    for n = -1 to 1 do
+      let n = d *. float n in
+      for e = -1 to 1 do
+	let e = d *. float e in
+	for d = 0 to 1 do
+	  let d = float (-100 * d) in
+	  let plane_ned = make_ned [| n; e; d|] in
+	  
+	  printf "    Plane NED: %a\n%!" fprint_ned plane_ned;
+	  let plane_ecef = ecef_of_ned_muret plane_ned in
+	  (* printf "Plane ECEF: %a\n%!" fprint_ecef plane_ecef; *)
+	  
+	  let plane_ned' = ned_of_ecef_muret plane_ecef in
+	  (* printf "Plane NED': %a\n%!" fprint_ned plane_ned'; *)
+	  assert (ned_eq plane_ned plane_ned');
+	  
+  (* Angles in UTM and NED *)
+	  let plane_geo, plane_h = geo_of_ecef WGS84 plane_ecef in
+	  printf "    Plane LLH: %s %.2f\n%!" (string_degrees_of_geographic plane_geo) plane_h;
+	  
+	  let plane_utm = utm_of WGS84 plane_geo in
+	  
+	  
+	  printf "    Plane UTM: %a\n%!" fprint_utm plane_utm;
+	  printf "Delta UTM [%.2f %.2f]\n%!" (plane_utm.utm_x -. muret_utm.utm_x -. e) (plane_utm.utm_y -. muret_utm.utm_y -. n);
+	  printf "Delta alt: %.2fm\n" (plane_h -. muret_h +. d)
+	done
+      done
+    done
+  done