Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

geodetic lattitude is compulsory for NED-ECEF conversions

Browse Source
This commit is contained in:
Pascal Brisset
2009-02-01 21:38:29 +00:00
parent b185f56b9a
commit 4ab5c895f0
2 changed files with 66 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/lib/ocaml/latlong.ml
+30 -32
View File
@@ -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|]
sw/lib/ocaml/test/test_latlong.ml
+36 -25
View File
@@ -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