Two-body Keplerian orbit propagation
Fix NaN propagation: satellites never produce non-finite positions
Found by fuzz tests. Fixes in Kepler + Globe.Satellite:
- Degenerate inputs fall back to RK4
- RK4: clamp steps, stop on NaN, guard non-finite dt
- Satellite: NaN output falls back to epoch position
- 8 unguarded fuzz tests all pass
+96
-73
+81
-63
lib/analytic.ml
+81
-63
lib/analytic.ml
···
27
27
argp : float; (** Argument of periapsis (rad). *)
28
28
nu : float; (** True anomaly (rad). *)
29
29
n : float; (** Mean motion (rad/s). *)
30
+
pos : Vec3.t; (** Original position (for RK4 fallback). *)
31
+
vel : Vec3.t; (** Original velocity (for RK4 fallback). *)
30
32
}
31
33
32
34
(** {1 State vector ↔ orbital elements} *)
···
36
38
let open Vec3 in
37
39
let r_mag = length pos in
38
40
let v_mag = length vel in
39
-
(* Angular momentum vector h = r × v *)
40
-
let h = cross pos vel in
41
-
let h_mag = length h in
42
-
(* Node vector n = k × h (k = [0,0,1]) *)
43
-
let n_vec = { x = -.h.y; y = h.x; z = 0. } in
44
-
let n_mag = length n_vec in
45
-
(* Eccentricity vector: e = (1/μ)[(v²-μ/r)r - (r·v)v] *)
46
-
let rdotv = dot pos vel in
47
-
let e_vec =
48
-
let term1 = scale ((v_mag *. v_mag) -. (mu /. r_mag)) pos in
49
-
let term2 = scale rdotv vel in
50
-
scale (1. /. mu) (add term1 (scale (-1.) term2))
51
-
in
52
-
let e = length e_vec in
53
-
(* Semi-major axis: a = -μ / (2ε) where ε = v²/2 - μ/r *)
54
-
let energy = (v_mag *. v_mag /. 2.) -. (mu /. r_mag) in
55
-
let a =
56
-
if abs_float energy < 1e-10 then r_mag (* parabolic fallback *)
57
-
else -.mu /. (2. *. energy)
58
-
in
59
-
(* Inclination: cos(i) = hz / |h| *)
60
-
let i = acos (Float.max (-1.) (Float.min 1. (h.z /. h_mag))) in
61
-
(* RAAN: cos(Ω) = nx / |n| *)
62
-
let raan =
63
-
if n_mag < 1e-10 then 0.
64
-
else
65
-
let cos_raan = n_vec.x /. n_mag in
66
-
let raan = acos (Float.max (-1.) (Float.min 1. cos_raan)) in
67
-
if n_vec.y < 0. then twopi -. raan else raan
68
-
in
69
-
(* Argument of periapsis: cos(ω) = n·e / (|n||e|) *)
70
-
let argp =
71
-
if n_mag < 1e-10 || e < 1e-10 then 0.
72
-
else
73
-
let cos_argp = dot n_vec e_vec /. (n_mag *. e) in
74
-
let argp = acos (Float.max (-1.) (Float.min 1. cos_argp)) in
75
-
if e_vec.z < 0. then twopi -. argp else argp
76
-
in
77
-
(* True anomaly: cos(ν) = e·r / (|e||r|) *)
78
-
let nu =
79
-
if e < 1e-10 then
80
-
(* Circular: use argument of latitude *)
81
-
let cos_u = dot n_vec pos /. (n_mag *. r_mag) in
82
-
let u = acos (Float.max (-1.) (Float.min 1. cos_u)) in
83
-
if pos.z < 0. then twopi -. u else u
84
-
else
85
-
let cos_nu = dot e_vec pos /. (e *. r_mag) in
86
-
let nu = acos (Float.max (-1.) (Float.min 1. cos_nu)) in
87
-
if rdotv < 0. then twopi -. nu else nu
88
-
in
89
-
(* Mean motion *)
90
-
let n =
91
-
if a > 0. then sqrt (mu /. (a *. a *. a))
92
-
else sqrt (mu /. (-.a *. -.a *. -.a))
93
-
(* hyperbolic *)
94
-
in
95
-
{ a; e; i; raan; argp; nu; n }
41
+
(* Guard degenerate inputs *)
42
+
if r_mag < 1e-10 || v_mag < 1e-10 then
43
+
{
44
+
a = r_mag;
45
+
e = 2.;
46
+
i = 0.;
47
+
raan = 0.;
48
+
argp = 0.;
49
+
nu = 0.;
50
+
n = 0.;
51
+
pos;
52
+
vel;
53
+
}
54
+
else
55
+
(* Angular momentum vector h = r × v *)
56
+
let h = cross pos vel in
57
+
let h_mag = length h in
58
+
(* Node vector n = k × h (k = [0,0,1]) *)
59
+
let n_vec = { x = -.h.y; y = h.x; z = 0. } in
60
+
let n_mag = length n_vec in
61
+
(* Eccentricity vector: e = (1/μ)[(v²-μ/r)r - (r·v)v] *)
62
+
let rdotv = dot pos vel in
63
+
let e_vec =
64
+
let term1 = scale ((v_mag *. v_mag) -. (mu /. r_mag)) pos in
65
+
let term2 = scale rdotv vel in
66
+
scale (1. /. mu) (add term1 (scale (-1.) term2))
67
+
in
68
+
let e = length e_vec in
69
+
(* Semi-major axis: a = -μ / (2ε) where ε = v²/2 - μ/r *)
70
+
let energy = (v_mag *. v_mag /. 2.) -. (mu /. r_mag) in
71
+
let a =
72
+
if abs_float energy < 1e-10 then r_mag (* parabolic fallback *)
73
+
else -.mu /. (2. *. energy)
74
+
in
75
+
(* Inclination: cos(i) = hz / |h| *)
76
+
let i = acos (Float.max (-1.) (Float.min 1. (h.z /. h_mag))) in
77
+
(* RAAN: cos(Ω) = nx / |n| *)
78
+
let raan =
79
+
if n_mag < 1e-10 then 0.
80
+
else
81
+
let cos_raan = n_vec.x /. n_mag in
82
+
let raan = acos (Float.max (-1.) (Float.min 1. cos_raan)) in
83
+
if n_vec.y < 0. then twopi -. raan else raan
84
+
in
85
+
(* Argument of periapsis: cos(ω) = n·e / (|n||e|) *)
86
+
let argp =
87
+
if n_mag < 1e-10 || e < 1e-10 then 0.
88
+
else
89
+
let cos_argp = dot n_vec e_vec /. (n_mag *. e) in
90
+
let argp = acos (Float.max (-1.) (Float.min 1. cos_argp)) in
91
+
if e_vec.z < 0. then twopi -. argp else argp
92
+
in
93
+
(* True anomaly: cos(ν) = e·r / (|e||r|) *)
94
+
let nu =
95
+
if e < 1e-10 then
96
+
(* Circular: use argument of latitude *)
97
+
let cos_u = dot n_vec pos /. (n_mag *. r_mag) in
98
+
let u = acos (Float.max (-1.) (Float.min 1. cos_u)) in
99
+
if pos.z < 0. then twopi -. u else u
100
+
else
101
+
let cos_nu = dot e_vec pos /. (e *. r_mag) in
102
+
let nu = acos (Float.max (-1.) (Float.min 1. cos_nu)) in
103
+
if rdotv < 0. then twopi -. nu else nu
104
+
in
105
+
(* Mean motion *)
106
+
let n =
107
+
if a > 0. then sqrt (mu /. (a *. a *. a))
108
+
else sqrt (mu /. (-.a *. -.a *. -.a))
109
+
(* hyperbolic *)
110
+
in
111
+
{ a; e; i; raan; argp; nu; n; pos; vel }
96
112
97
113
(** {1 Kepler's equation solver} *)
98
114
···
172
188
use {!at_precomputed} for each time step. *)
173
189
let precompute ~pos ~vel = elements_of_state pos vel
174
190
175
-
(** Propagate from precomputed elements. Very fast (~20 FLOPs). *)
191
+
(** Propagate from precomputed elements. Very fast (~20 FLOPs). Falls back to
192
+
RK4 for hyperbolic/parabolic orbits. *)
176
193
let at_precomputed elements ~dt =
177
-
if elements.e >= 1.0 then
178
-
(* Can't use analytic for hyperbolic — should not happen if
179
-
precompute was called on a bound orbit *)
180
-
Vec3.zero
181
-
else position_at_dt elements dt
194
+
if elements.e >= 1.0 then Propagate.at ~pos:elements.pos ~vel:elements.vel ~dt
195
+
else
196
+
let p = position_at_dt elements dt in
197
+
(* Guard against numerical issues near singularities *)
198
+
if Float.is_finite p.x && Float.is_finite p.y && Float.is_finite p.z then p
199
+
else Propagate.at ~pos:elements.pos ~vel:elements.vel ~dt
182
200
183
201
(** Generate an arc of [num_points] positions centered on epoch. *)
184
202
let arc ~pos ~vel ~duration_s ~num_points =
+15
-10
lib/propagate.ml
+15
-10
lib/propagate.ml
···
38
38
(new_pos, new_vel)
39
39
40
40
let at ~pos ~vel ~dt =
41
-
(* Use ~30s steps for accuracy *)
42
-
let n_steps = Float.to_int (abs_float dt /. 30.) + 1 in
43
-
let step = dt /. Float.of_int n_steps in
44
-
let p = ref pos and v = ref vel in
45
-
for _ = 1 to n_steps do
46
-
let p', v' = rk4_step !p !v step in
47
-
p := p';
48
-
v := v'
49
-
done;
50
-
!p
41
+
if not (Float.is_finite dt) then pos
42
+
else
43
+
(* Clamp to max 10000 steps to prevent runaway *)
44
+
let n_steps = min 10000 (Float.to_int (abs_float dt /. 30.) + 1) in
45
+
let step = dt /. Float.of_int n_steps in
46
+
let p = ref pos and v = ref vel in
47
+
for _ = 1 to n_steps do
48
+
let p', v' = rk4_step !p !v step in
49
+
(* Guard against NaN propagation *)
50
+
if Float.is_finite p'.x then begin
51
+
p := p';
52
+
v := v'
53
+
end
54
+
done;
55
+
!p
51
56
52
57
let arc ~pos ~vel ~duration_s ~num_points =
53
58
let dt = duration_s /. Float.of_int num_points in