Astrodynamics coordinate frame transforms
Add TEME-to-J2000 frame conversion; fix station contacts; SGP4 bug found
ocaml-coordinate: Add teme_to_j2000_at and j2000_to_teme_at using IAU-76
precession + IAU-1980 nutation (top 13 terms covering 99.9% of signal).
Proper rotation chain: TEME -> TOD (undo nutation) -> MOD -> J2000 (undo
precession).
ocaml-sgp4 interop:
- Fix station contacts report path (GMAT writes to its output/ dir)
- 14 contact windows from LA over 3 days committed as trace
- SGP4-vs-GMAT now uses TEME->J2000 conversion before comparison
- SGP4 STILL fails: 42.8 km error after 14 hours. This is NOT a frame
issue (conversion only improved by 0.2 km). Genuine SGP4 implementation
difference — needs investigation (likely WGS72/WGS84 or B* model).
+102
-69
+95
-69
lib/coordinate.ml
+95
-69
lib/coordinate.ml
···
56
56
let j2000_to_ecef ~gmst v = rotate_z gmst v
57
57
let ecef_to_j2000 ~gmst v = rotate_z (-.gmst) v
58
58
59
-
(* ── TEME ↔ J2000 (IAU-76/FK5 precession + simplified nutation) ──── *)
59
+
(* ── TEME ↔ J2000 (IAU-76/FK5) ────────────────────────────────────── *)
60
+
61
+
let rotate_x theta (v : Vec3.t) =
62
+
let c = cos theta and s = sin theta in
63
+
Vec3.v v.x ((c *. v.y) +. (s *. v.z)) ((-.s *. v.y) +. (c *. v.z))
64
+
65
+
let rotate_y theta (v : Vec3.t) =
66
+
let c = cos theta and s = sin theta in
67
+
Vec3.v ((c *. v.x) -. (s *. v.z)) v.y ((s *. v.x) +. (c *. v.z))
68
+
69
+
let arcsec_to_rad a = a *. Float.pi /. (180. *. 3600.)
60
70
61
-
(* Precession angles (IAU-76, Lieske 1979).
62
-
T is Julian centuries from J2000.0.
63
-
Returns (zeta_a, theta_a, z_a) in radians. *)
71
+
(* IAU-76 precession angles (Lieske 1979). T = Julian centuries from J2000. *)
64
72
let precession_angles t =
65
-
let arcsec_to_rad a = a *. Float.pi /. (180. *. 3600.) in
66
73
let zeta_a =
67
74
arcsec_to_rad
68
75
((0.6406161 *. t) +. (0.0000839 *. t *. t) +. (0.0000050 *. t *. t *. t))
···
79
86
80
87
(* Mean obliquity of the ecliptic (IAU-76). *)
81
88
let mean_obliquity t =
82
-
let arcsec_to_rad a = a *. Float.pi /. (180. *. 3600.) in
83
89
deg_to_rad 23.439291
84
90
-. arcsec_to_rad
85
91
((46.8150 *. t) +. (0.00059 *. t *. t) -. (0.001813 *. t *. t *. t))
86
92
87
-
(* Simplified nutation (only the dominant 18.6-year lunar node term).
88
-
Returns (dpsi, deps) in radians. *)
89
-
let nutation_simplified t =
90
-
let arcsec_to_rad a = a *. Float.pi /. (180. *. 3600.) in
91
-
(* Longitude of lunar ascending node *)
92
-
let omega = deg_to_rad (125.04452 -. (1934.136261 *. t)) in
93
-
let dpsi = arcsec_to_rad (-17.20 *. sin omega) in
94
-
let deps = arcsec_to_rad (9.20 *. cos omega) in
95
-
(dpsi, deps)
96
-
97
-
let rotate_x theta (v : Vec3.t) =
98
-
let c = cos theta and s = sin theta in
99
-
Vec3.v v.x ((c *. v.y) +. (s *. v.z)) ((-.s *. v.y) +. (c *. v.z))
93
+
(* IAU-1980 nutation: top 13 terms covering 99.9% of the signal.
94
+
Full model has 106 terms; remaining 93 contribute < 0.1 arcsec total.
95
+
Coefficients from ERFA/SOFA nut80.c in 0.1 mas units.
96
+
Fundamental arguments: l, l', F, D, Omega (Delaunay variables). *)
97
+
let nutation_dpsi_deps t =
98
+
let t2 = t *. t in
99
+
let t3 = t2 *. t in
100
+
(* Fundamental arguments (arcsec, mod 360 deg → radians) *)
101
+
let to_rad a =
102
+
let a_deg = Float.rem (a /. 3600.0) 360.0 in
103
+
deg_to_rad (if a_deg < 0.0 then a_deg +. 360.0 else a_deg)
104
+
in
105
+
let el = to_rad (485866.733 +. ((1325.0 *. 360.0 *. 3600.0) +. 715922.633) *. t
106
+
+. 31.310 *. t2 +. 0.064 *. t3) in
107
+
let elp = to_rad (1287099.804 +. ((99.0 *. 360.0 *. 3600.0) +. 1292581.224) *. t
108
+
-. 0.577 *. t2 -. 0.012 *. t3) in
109
+
let f = to_rad (335778.877 +. ((1342.0 *. 360.0 *. 3600.0) +. 295263.137) *. t
110
+
-. 13.257 *. t2 +. 0.011 *. t3) in
111
+
let d = to_rad (1072261.307 +. ((1236.0 *. 360.0 *. 3600.0) +. 1105601.328) *. t
112
+
-. 6.891 *. t2 +. 0.019 *. t3) in
113
+
let om = to_rad (450160.280 +. ((-5.0 *. 360.0 *. 3600.0) -. 482890.539) *. t
114
+
+. 7.455 *. t2 +. 0.008 *. t3) in
115
+
(* Top 13 terms: nl, nlp, nf, nd, nom, sp (0.1mas), spt, ce, cet *)
116
+
let terms = [|
117
+
(* 1 *) (0,0,0,0,1, -171996.0, -174.2, 92025.0, 8.9);
118
+
(* 9 *) (0,0,2,-2,2, -13187.0, -1.6, 5736.0, -3.1);
119
+
(* 31 *) (0,0,2,0,2, -2274.0, -0.2, 977.0, -0.5);
120
+
(* 2 *) (0,0,0,0,2, 2062.0, 0.2, -895.0, 0.5);
121
+
(* 10 *) (0,1,0,0,0, 1426.0, -3.4, 54.0, -0.1);
122
+
(* 32 *) (1,0,0,0,0, 712.0, 0.1, -7.0, 0.0);
123
+
(* 11 *) (0,1,2,-2,2, -517.0, 1.2, 224.0, -0.6);
124
+
(* 33 *) (0,0,2,0,1, -386.0, -0.4, 200.0, 0.0);
125
+
(* 34 *) (1,0,2,0,2, -301.0, 0.0, 129.0, -0.1);
126
+
(* 12 *) (0,-1,2,-2,2, 217.0, -0.5, -95.0, 0.3);
127
+
(* 35 *) (1,0,0,-2,0, -158.0, 0.0, -1.0, 0.0);
128
+
(* 13 *) (0,0,2,-2,1, 129.0, 0.1, -70.0, 0.0);
129
+
(* 36 *) (-1,0,2,0,2, 123.0, 0.0, -53.0, 0.0);
130
+
|] in
131
+
let dpsi = ref 0.0 in
132
+
let deps = ref 0.0 in
133
+
Array.iter (fun (nl, nlp, nf, nd, nom, sp, spt, ce, cet) ->
134
+
let arg = (Float.of_int nl *. el) +. (Float.of_int nlp *. elp)
135
+
+. (Float.of_int nf *. f) +. (Float.of_int nd *. d)
136
+
+. (Float.of_int nom *. om) in
137
+
dpsi := !dpsi +. ((sp +. spt *. t) *. sin arg);
138
+
deps := !deps +. ((ce +. cet *. t) *. cos arg)
139
+
) terms;
140
+
(* Convert from 0.1 mas to radians *)
141
+
let mas_to_rad x = x *. 1e-4 *. Float.pi /. (180.0 *. 3600.0) in
142
+
(mas_to_rad !dpsi, mas_to_rad !deps)
100
143
101
-
(* Equation of the equinoxes: GMST → GAST correction *)
144
+
(* Equation of the equinoxes: GMST to GAST correction. *)
102
145
let eq_equinoxes t =
103
-
let dpsi, _deps = nutation_simplified t in
146
+
let dpsi, _deps = nutation_dpsi_deps t in
147
+
dpsi *. cos (mean_obliquity t)
148
+
149
+
(* Apply precession: MOD → J2000. P^T = Rz(zeta) * Ry(-theta) * Rz(z) *)
150
+
let precess_mod_to_j2000 t v =
151
+
let zeta_a, theta_a, z_a = precession_angles t in
152
+
rotate_z zeta_a (rotate_y (-.theta_a) (rotate_z z_a v))
153
+
154
+
(* Apply precession: J2000 → MOD. P = Rz(-z) * Ry(theta) * Rz(-zeta) *)
155
+
let precess_j2000_to_mod t v =
156
+
let zeta_a, theta_a, z_a = precession_angles t in
157
+
rotate_z (-.z_a) (rotate_y theta_a (rotate_z (-.zeta_a) v))
158
+
159
+
(* Apply nutation: TOD → MOD. N^T = Rx(-eps) * Rz(dpsi) * Rx(eps+deps) *)
160
+
let nutate_tod_to_mod t v =
161
+
let dpsi, deps = nutation_dpsi_deps t in
162
+
let eps = mean_obliquity t in
163
+
rotate_x (-.eps) (rotate_z dpsi (rotate_x (eps +. deps) v))
164
+
165
+
(* Apply nutation: MOD → TOD. N = Rx(-eps-deps) * Rz(-dpsi) * Rx(eps) *)
166
+
let nutate_mod_to_tod t v =
167
+
let dpsi, deps = nutation_dpsi_deps t in
104
168
let eps = mean_obliquity t in
105
-
dpsi *. cos eps
169
+
rotate_x (-.(eps +. deps)) (rotate_z (-.dpsi) (rotate_x eps v))
106
170
171
+
(* TEME → J2000: TEME is approximately TOD with mean equinox.
172
+
The difference between TEME and TOD is the equation of equinoxes applied
173
+
to the right ascension only. For SGP4 precision, TEME ≈ TOD.
174
+
Chain: TEME ≈ TOD → MOD (undo nutation) → J2000 (undo precession). *)
107
175
let teme_to_j2000_at ~unix_t v =
108
176
let t = julian_centuries unix_t in
109
-
(* Step 1: TEME → PEF (pseudo Earth-fixed, like ECEF but without polar motion)
110
-
Undo Earth rotation using GAST = GMST + equation of equinoxes *)
111
-
let gmst = gmst_of_unix unix_t in
112
-
let gast = gmst +. eq_equinoxes t in
113
-
let pef = rotate_z gast v in
114
-
(* Step 2: PEF → J2000 via inverse precession+nutation *)
115
-
let dpsi, deps = nutation_simplified t in
116
-
let eps = mean_obliquity t in
117
-
let eps_true = eps +. deps in
118
-
(* Nutation: N^T = Rx(eps_true) * Rz(dpsi) * Rx(-eps) *)
119
-
let v1 = rotate_x (-.eps) pef in
120
-
let v2 = rotate_z dpsi v1 in
121
-
let v3 = rotate_x eps_true v2 in
122
-
(* Precession: P^T = Rz(z_a) * Ry(-theta_a) * Rz(zeta_a) *)
123
-
let zeta_a, theta_a, z_a = precession_angles t in
124
-
let v4 = rotate_z zeta_a v3 in
125
-
let v5 = rotate_x 0.0 v4 in
126
-
let _ = v5 in
127
-
(* Actually: P = Rz(-z_a) * Ry(theta_a) * Rz(-zeta_a)
128
-
P^T (mod-to-J2000) = Rz(zeta_a) * Ry(-theta_a) * Rz(z_a) *)
129
-
let rotate_y theta (v : Vec3.t) =
130
-
let c = cos theta and s = sin theta in
131
-
Vec3.v ((c *. v.x) -. (s *. v.z)) v.y ((s *. v.x) +. (c *. v.z))
132
-
in
133
-
let v4 = rotate_z z_a v3 in
134
-
let v5 = rotate_y (-.theta_a) v4 in
135
-
rotate_z zeta_a v5
177
+
(* TEME ≈ TOD for SGP4 precision *)
178
+
let mod_ = nutate_tod_to_mod t v in
179
+
precess_mod_to_j2000 t mod_
136
180
137
181
let j2000_to_teme_at ~unix_t v =
138
182
let t = julian_centuries unix_t in
139
-
let zeta_a, theta_a, z_a = precession_angles t in
140
-
let rotate_y theta (v : Vec3.t) =
141
-
let c = cos theta and s = sin theta in
142
-
Vec3.v ((c *. v.x) -. (s *. v.z)) v.y ((s *. v.x) +. (c *. v.z))
143
-
in
144
-
(* Precession: P = Rz(-z_a) * Ry(theta_a) * Rz(-zeta_a) *)
145
-
let v1 = rotate_z (-.zeta_a) v in
146
-
let v2 = rotate_y theta_a v1 in
147
-
let v3 = rotate_z (-.z_a) v2 in
148
-
(* Nutation: N = Rx(-eps_true) * Rz(-dpsi) * Rx(eps) *)
149
-
let dpsi, deps = nutation_simplified t in
150
-
let eps = mean_obliquity t in
151
-
let eps_true = eps +. deps in
152
-
let v4 = rotate_x eps v3 in
153
-
let v5 = rotate_z (-.dpsi) v4 in
154
-
let v6 = rotate_x (-.eps_true) v5 in
155
-
(* Earth rotation: GAST *)
156
-
let gmst = gmst_of_unix unix_t in
157
-
let gast = gmst +. eq_equinoxes t in
158
-
rotate_z (-.gast) v6
183
+
let mod_ = precess_j2000_to_mod t v in
184
+
nutate_mod_to_tod t mod_
159
185
160
186
(* ── Geodetic conversions ──────────────────────────────────────────── *)
161
187
+7
lib/coordinate.mli
+7
lib/coordinate.mli
···
40
40
val ecef_to_teme : gmst:float -> Vec3.t -> Vec3.t
41
41
val j2000_to_ecef : gmst:float -> Vec3.t -> Vec3.t
42
42
val ecef_to_j2000 : gmst:float -> Vec3.t -> Vec3.t
43
+
val teme_to_j2000_at : unix_t:float -> Vec3.t -> Vec3.t
44
+
(** [teme_to_j2000_at ~unix_t v] converts from TEME (SGP4 output frame) to
45
+
J2000/EME2000 using IAU-76 precession and IAU-1980 nutation (top 13 terms).
46
+
[unix_t] is the Unix timestamp at which the conversion applies. *)
47
+
48
+
val j2000_to_teme_at : unix_t:float -> Vec3.t -> Vec3.t
49
+
(** [j2000_to_teme_at ~unix_t v] converts from J2000/EME2000 to TEME. *)
43
50
44
51
(** {1 Geodetic conversions} *)
45
52