300 lines
11 KiB
Rust
300 lines
11 KiB
Rust
//! The near-Earth propagation step.
|
|
//!
|
|
//! Given a satellite record and a time from epoch, this module evaluates the
|
|
//! secular and periodic terms of the SGP4 theory and returns the state in
|
|
//! the True Equator Mean Equinox frame.
|
|
//!
|
|
//! The mean elements advance as polynomials in time: the mean anomaly,
|
|
//! argument of perigee, and right ascension are linear, the right ascension
|
|
//! has an additional quadratic term, and the semi-major axis, eccentricity,
|
|
//! and mean anomaly pick up further drag terms for orbits above the
|
|
//! simplified threshold. The orbit is then solved for the eccentric anomaly
|
|
//! by Newton's method, and the short period terms are applied.
|
|
//!
|
|
//! Source: the sequence follows Vallado et al., *Revisiting Spacetrack
|
|
//! Report #3*, AIAA 2006-6753, 2006, and the reference implementation
|
|
//! distributed with that paper.
|
|
|
|
use crate::error::{PropagationError, PropagationErrorKind};
|
|
use crate::satellite::{Method, Satellite, TWO_PI};
|
|
use crate::state::State;
|
|
use apsis_math::Vec3;
|
|
|
|
/// The number of iterations allowed when solving for the eccentric anomaly.
|
|
const KEPLER_MAX_ITERATIONS: usize = 10;
|
|
|
|
/// The convergence threshold on the eccentric anomaly correction.
|
|
const KEPLER_TOLERANCE: f64 = 1.0e-12;
|
|
|
|
/// The largest correction applied in one Kepler step.
|
|
///
|
|
/// Clamping keeps the iteration stable for the eccentric orbits that the
|
|
/// near circular approximation of the theory handles poorly.
|
|
const KEPLER_MAX_STEP: f64 = 0.95;
|
|
|
|
/// The smallest eccentricity used after the drag terms are applied.
|
|
///
|
|
/// A value that would round to zero makes several divisions in the short
|
|
/// period terms singular, so it is clamped to this floor.
|
|
const MINIMUM_ECCENTRICITY: f64 = 1.0e-6;
|
|
|
|
/// The divisor used when the inclination is close to pi.
|
|
const INCLINATION_GUARD: f64 = 1.5e-12;
|
|
|
|
/// Propagates a satellite by a number of minutes from its element epoch.
|
|
pub(crate) fn propagate(satellite: &Satellite, minutes: f64) -> Result<State, PropagationError> {
|
|
let mean_anomaly = satellite.mo + satellite.mdot * minutes;
|
|
let secular_argument = satellite.argpo + satellite.argpdot * minutes;
|
|
let secular_node = satellite.nodeo + satellite.nodedot * minutes;
|
|
|
|
let time_squared = minutes * minutes;
|
|
let mut node = secular_node + satellite.nodecf * time_squared;
|
|
|
|
let mut tempa = 1.0 - satellite.cc1 * minutes;
|
|
let mut tempe = satellite.bstar * satellite.cc4 * minutes;
|
|
let mut templ = satellite.t2cof * time_squared;
|
|
|
|
let mut argument = secular_argument;
|
|
let mut anomaly = mean_anomaly;
|
|
|
|
if !satellite.isimp {
|
|
let delta_argument = satellite.omgcof * minutes;
|
|
let drag_anomaly = 1.0 + satellite.eta * mean_anomaly.cos();
|
|
let delta_anomaly =
|
|
satellite.xmcof * (drag_anomaly * drag_anomaly * drag_anomaly - satellite.delmo);
|
|
let total = delta_argument + delta_anomaly;
|
|
anomaly = mean_anomaly + total;
|
|
argument = secular_argument - total;
|
|
let time_cubed = time_squared * minutes;
|
|
let time_fourth = time_cubed * minutes;
|
|
tempa -=
|
|
satellite.d2 * time_squared + satellite.d3 * time_cubed + satellite.d4 * time_fourth;
|
|
tempe += satellite.bstar * satellite.cc5 * (anomaly.sin() - satellite.sinmao);
|
|
templ = templ
|
|
+ satellite.t3cof * time_cubed
|
|
+ time_fourth * (satellite.t4cof + minutes * satellite.t5cof);
|
|
}
|
|
|
|
let mut eccentricity = satellite.ecco;
|
|
let mut inclination = satellite.inclo;
|
|
let mut mean_motion = satellite.no_unkozai;
|
|
|
|
// The deep space branch replaces the mean elements here.
|
|
if satellite.method == Method::DeepSpace {
|
|
let deep = crate::deep_space::dspace(
|
|
satellite,
|
|
minutes,
|
|
eccentricity,
|
|
argument,
|
|
inclination,
|
|
anomaly,
|
|
node,
|
|
mean_motion,
|
|
);
|
|
eccentricity = deep.eccentricity;
|
|
argument = deep.argument;
|
|
inclination = deep.inclination;
|
|
anomaly = deep.mean_anomaly;
|
|
node = deep.node;
|
|
mean_motion = deep.mean_motion;
|
|
}
|
|
|
|
if mean_motion <= 0.0 {
|
|
return Err(super::satellite::non_positive_mean_motion(mean_motion));
|
|
}
|
|
|
|
// Recover the semi-major axis including the drag scaling, then recompute
|
|
// the mean motion from it so the two stay consistent.
|
|
let semi_major = (satellite.xke() / mean_motion).powf(2.0 / 3.0) * tempa * tempa;
|
|
mean_motion = satellite.xke() / semi_major.powf(1.5);
|
|
|
|
eccentricity -= tempe;
|
|
if eccentricity >= 1.0 || eccentricity < -0.001 {
|
|
return Err(PropagationError::new(PropagationErrorKind::InvalidElements)
|
|
.with_detail(format!("propagated eccentricity is {eccentricity}")));
|
|
}
|
|
let eccentricity = eccentricity.max(MINIMUM_ECCENTRICITY);
|
|
|
|
anomaly += satellite.no_unkozai * templ;
|
|
let mut longitude = anomaly + argument + node;
|
|
|
|
// Reduce the angles to a single turn. The reference implementation uses
|
|
// a remainder that keeps the sign of the dividend for the right
|
|
// ascension and a positive remainder elsewhere; both are reproduced here
|
|
// because the eccentric anomaly iteration is sensitive to the branch.
|
|
node = if node >= 0.0 {
|
|
node % TWO_PI
|
|
} else {
|
|
-((-node) % TWO_PI)
|
|
};
|
|
argument %= TWO_PI;
|
|
longitude %= TWO_PI;
|
|
anomaly = (longitude - argument - node) % TWO_PI;
|
|
|
|
let mut perturbed_eccentricity = eccentricity;
|
|
let mut perturbed_inclination = inclination;
|
|
let mut perturbed_node = node;
|
|
let mut perturbed_argument = argument;
|
|
let mut perturbed_anomaly = anomaly;
|
|
let mut sin_perturbed = inclination.sin();
|
|
let mut cos_perturbed = inclination.cos();
|
|
|
|
if satellite.method == Method::DeepSpace {
|
|
let result = crate::deep_space::dpper(
|
|
satellite,
|
|
minutes,
|
|
false,
|
|
perturbed_eccentricity,
|
|
perturbed_inclination,
|
|
perturbed_node,
|
|
perturbed_argument,
|
|
perturbed_anomaly,
|
|
);
|
|
perturbed_eccentricity = result.eccentricity;
|
|
perturbed_inclination = result.inclination;
|
|
perturbed_node = result.node;
|
|
perturbed_argument = result.argument;
|
|
perturbed_anomaly = result.mean_anomaly;
|
|
|
|
if perturbed_inclination < 0.0 {
|
|
perturbed_inclination = -perturbed_inclination;
|
|
perturbed_node += core::f64::consts::PI;
|
|
perturbed_argument -= core::f64::consts::PI;
|
|
}
|
|
if perturbed_eccentricity < 0.0 || perturbed_eccentricity > 1.0 {
|
|
return Err(
|
|
PropagationError::new(PropagationErrorKind::InvalidElements).with_detail(format!(
|
|
"perturbed eccentricity is {perturbed_eccentricity}"
|
|
)),
|
|
);
|
|
}
|
|
|
|
sin_perturbed = perturbed_inclination.sin();
|
|
cos_perturbed = perturbed_inclination.cos();
|
|
}
|
|
|
|
// The `aycof` and `xlcof` used below come from the perturbed
|
|
// inclination in the deep space branch and from the epoch inclination
|
|
// otherwise.
|
|
let (aycof, xlcof) = if satellite.method == Method::DeepSpace {
|
|
let aycof = -0.5 * satellite.model.j3_over_j2() * sin_perturbed;
|
|
let xlcof = if (cos_perturbed + 1.0).abs() > INCLINATION_GUARD {
|
|
-0.25 * satellite.model.j3_over_j2() * sin_perturbed * (3.0 + 5.0 * cos_perturbed)
|
|
/ (1.0 + cos_perturbed)
|
|
} else {
|
|
-0.25 * satellite.model.j3_over_j2() * sin_perturbed * (3.0 + 5.0 * cos_perturbed)
|
|
/ INCLINATION_GUARD
|
|
};
|
|
(aycof, xlcof)
|
|
} else {
|
|
(satellite.aycof, satellite.xlcof)
|
|
};
|
|
|
|
let axnl = perturbed_eccentricity * perturbed_argument.cos();
|
|
let temp = 1.0 / (semi_major * (1.0 - perturbed_eccentricity * perturbed_eccentricity));
|
|
let aynl = perturbed_eccentricity * perturbed_argument.sin() + temp * aycof;
|
|
let xl = perturbed_anomaly + perturbed_argument + perturbed_node + temp * xlcof * axnl;
|
|
|
|
let u = (xl - perturbed_node) % TWO_PI;
|
|
|
|
// Solve Kepler's equation by Newton's method.
|
|
let mut eo1 = u;
|
|
let (mut sineo1, mut coseo1);
|
|
let mut iterations = 0;
|
|
loop {
|
|
sineo1 = eo1.sin();
|
|
coseo1 = eo1.cos();
|
|
let denominator = 1.0 - coseo1 * axnl - sineo1 * aynl;
|
|
let mut correction = (u - aynl * coseo1 + axnl * sineo1 - eo1) / denominator;
|
|
if correction.abs() >= KEPLER_MAX_STEP {
|
|
correction = if correction > 0.0 {
|
|
KEPLER_MAX_STEP
|
|
} else {
|
|
-KEPLER_MAX_STEP
|
|
};
|
|
}
|
|
eo1 += correction;
|
|
iterations += 1;
|
|
if correction.abs() < KEPLER_TOLERANCE || iterations >= KEPLER_MAX_ITERATIONS {
|
|
break;
|
|
}
|
|
}
|
|
sineo1 = eo1.sin();
|
|
coseo1 = eo1.cos();
|
|
|
|
let ecose = axnl * coseo1 + aynl * sineo1;
|
|
let esine = axnl * sineo1 - aynl * coseo1;
|
|
let el2 = axnl * axnl + aynl * aynl;
|
|
let pl = semi_major * (1.0 - el2);
|
|
if pl < 0.0 {
|
|
return Err(PropagationError::new(PropagationErrorKind::InvalidElements)
|
|
.with_detail(format!("semi-latus rectum is {pl}")));
|
|
}
|
|
|
|
let rl = semi_major * (1.0 - ecose);
|
|
let rdotl = semi_major.sqrt() * esine / rl;
|
|
let rvdotl = pl.sqrt() / rl;
|
|
let betal = (1.0 - el2).sqrt();
|
|
let temp = esine / (1.0 + betal);
|
|
let sinu = semi_major / rl * (sineo1 - aynl - axnl * temp);
|
|
let cosu = semi_major / rl * (coseo1 - axnl + aynl * temp);
|
|
let mut su = sinu.atan2(cosu);
|
|
let sin2u = (cosu + cosu) * sinu;
|
|
let cos2u = 1.0 - 2.0 * sinu * sinu;
|
|
|
|
let temp = 1.0 / pl;
|
|
let temp1 = 0.5 * satellite.model.j2 * temp;
|
|
let temp2 = temp1 * temp;
|
|
|
|
let (con41, x1mth2, x7thm1) = if satellite.method == Method::DeepSpace {
|
|
let cosisq = cos_perturbed * cos_perturbed;
|
|
(3.0 * cosisq - 1.0, 1.0 - cosisq, 7.0 * cosisq - 1.0)
|
|
} else {
|
|
(satellite.con41, satellite.x1mth2, satellite.x7thm1)
|
|
};
|
|
|
|
let mrt = rl * (1.0 - 1.5 * temp2 * betal * con41) + 0.5 * temp1 * x1mth2 * cos2u;
|
|
su -= 0.25 * temp2 * x7thm1 * sin2u;
|
|
let xnode = perturbed_node + 1.5 * temp2 * cos_perturbed * sin2u;
|
|
let xinc = perturbed_inclination + 1.5 * temp2 * cos_perturbed * sin_perturbed * cos2u;
|
|
|
|
let mvt = rdotl - mean_motion * temp1 * x1mth2 * sin2u / satellite.xke();
|
|
let rvdot = rvdotl + mean_motion * temp1 * (x1mth2 * cos2u + 1.5 * con41) / satellite.xke();
|
|
|
|
let sinsu = su.sin();
|
|
let cossu = su.cos();
|
|
let snod = xnode.sin();
|
|
let cnod = xnode.cos();
|
|
let sini = xinc.sin();
|
|
let cosi = xinc.cos();
|
|
|
|
let xmx = -snod * cosi;
|
|
let xmy = cnod * cosi;
|
|
let ux = xmx * sinsu + cnod * cossu;
|
|
let uy = xmy * sinsu + snod * cossu;
|
|
let uz = sini * sinsu;
|
|
let vx = xmx * cossu - cnod * sinsu;
|
|
let vy = xmy * cossu - snod * sinsu;
|
|
let vz = sini * cossu;
|
|
|
|
let radius = mrt * satellite.model.radius;
|
|
let position = Vec3::new(radius * ux, radius * uy, radius * uz);
|
|
let velocity_scale = satellite.model.radius * satellite.xke() / 60.0;
|
|
let velocity = Vec3::new(
|
|
(mvt * ux + rvdot * vx) * velocity_scale,
|
|
(mvt * uy + rvdot * vy) * velocity_scale,
|
|
(mvt * uz + rvdot * vz) * velocity_scale,
|
|
);
|
|
|
|
if mrt < 1.0 {
|
|
return Err(PropagationError::new(PropagationErrorKind::Decayed)
|
|
.with_detail(format!("radius is {mrt} Earth radii")));
|
|
}
|
|
|
|
Ok(State {
|
|
position,
|
|
velocity,
|
|
semi_major_axis: semi_major * satellite.model.radius,
|
|
})
|
|
}
|