#![allow(
clippy::needless_range_loop,
clippy::many_single_char_names,
clippy::similar_names
)]
#[derive(Debug, Clone, Copy)]
pub struct UnscentedParams {
pub alpha: f64,
pub beta: f64,
pub kappa: f64,
}
impl UnscentedParams {
#[must_use]
pub const fn new(alpha: f64, beta: f64, kappa: f64) -> Self {
Self { alpha, beta, kappa }
}
#[must_use]
pub const fn gaussian() -> Self {
Self {
alpha: 1e-3,
beta: 2.0,
kappa: 0.0,
}
}
#[must_use]
#[allow(clippy::cast_precision_loss)] pub(crate) fn weights<const N: usize>(&self) -> Weights {
let n = N as f64;
let lambda = self.alpha * self.alpha * (n + self.kappa) - n;
let c = n + lambda;
assert!(
c > 0.0,
"unscented scaling N + λ must be positive (check alpha, kappa)"
);
let w0m = lambda / c;
let wi = 1.0 / (2.0 * c);
Weights {
gamma: libm::sqrt(c),
w0m,
w0c: w0m + (1.0 - self.alpha * self.alpha + self.beta),
wi,
}
}
}
#[derive(Debug, Clone, Copy)]
pub(crate) struct Weights {
pub gamma: f64,
pub w0m: f64,
pub w0c: f64,
pub wi: f64,
}
#[must_use]
pub(crate) fn sigma_points<const N: usize, const L: usize>(
mean: &[f64; N],
sqrt: &[[f64; N]; N],
gamma: f64,
) -> [[f64; N]; L] {
assert!(L == 2 * N + 1, "sigma-point count L must equal 2N + 1");
let mut pts = [[0.0; N]; L];
pts[0] = *mean;
for k in 0..N {
let mut plus = *mean;
let mut minus = *mean;
for i in 0..N {
let d = gamma * sqrt[i][k];
plus[i] += d;
minus[i] -= d;
}
pts[1 + k] = plus;
pts[1 + N + k] = minus;
}
pts
}
pub(crate) fn centered_residuals<const M: usize, const L: usize>(
cloud: &[[f64; M]; L],
w: &Weights,
) -> ([f64; M], [[f64; M]; L]) {
#[allow(clippy::cast_precision_loss)] let off_center = (L - 1) as f64;
debug_assert!(
libm::fabs(w.w0m + off_center * w.wi - 1.0)
<= 8.0 * f64::EPSILON * libm::fmax(libm::fabs(w.w0m), 1.0),
"the mean weights must sum to one"
);
let wi = w.wi;
let mut resid = [[0.0; M]; L];
for i in 1..L {
for j in 0..M {
resid[i][j] = cloud[i][j] - cloud[0][j];
}
}
let mut offset = [0.0; M];
for i in 1..L {
for j in 0..M {
offset[j] += wi * resid[i][j];
}
}
for i in 1..L {
for j in 0..M {
resid[i][j] -= offset[j];
}
}
for j in 0..M {
resid[0][j] = -offset[j];
}
(offset, resid)
}
#[must_use]
pub fn unscented_transform<const N: usize, const D: usize, const L: usize>(
params: &UnscentedParams,
mean: &[f64; N],
sqrt: &[[f64; N]; N],
map: impl Fn(&[f64; N]) -> [f64; D],
) -> ([f64; D], [[f64; D]; D]) {
let w = params.weights::<N>();
let pts = sigma_points::<N, L>(mean, sqrt, w.gamma);
let mut y = [[0.0; D]; L];
for i in 0..L {
y[i] = map(&pts[i]);
}
let (offset, resid) = centered_residuals::<D, L>(&y, &w);
let mut ybar = [0.0; D];
for j in 0..D {
ybar[j] = y[0][j] + offset[j];
}
let mut cov = [[0.0; D]; D];
for i in 0..L {
let weight = if i == 0 { w.w0c } else { w.wi };
for a in 0..D {
for b in 0..D {
cov[a][b] += weight * resid[i][a] * resid[i][b];
}
}
}
(ybar, cov)
}
#[must_use]
pub(crate) fn cholesky<const N: usize>(a: &[[f64; N]; N]) -> Option<[[f64; N]; N]> {
let mut l = [[0.0; N]; N];
for i in 0..N {
for j in 0..=i {
let mut sum = a[i][j];
for k in 0..j {
sum -= l[i][k] * l[j][k];
}
if i == j {
if sum <= 0.0 {
return None;
}
l[i][j] = libm::sqrt(sum);
} else {
l[i][j] = sum / l[j][j];
}
}
}
Some(l)
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp)]
use super::*;
use spacelab_testkit::nan_max;
const SETS: [(f64, f64, f64); 5] = [
(1e-3, 2.0, 0.0),
(0.9, 2.0, 1.0),
(1.0, 0.0, -1.0),
(0.5, 1.0, 3.0),
(0.25, 2.0, 2.0),
];
#[test]
fn the_mean_weights_sum_to_one() {
for (alpha, beta, kappa) in SETS {
let p = UnscentedParams::new(alpha, beta, kappa);
let (w3, w6) = (p.weights::<3>(), p.weights::<6>());
let (s3, s6) = (w3.w0m + 6.0 * w3.wi - 1.0, w6.w0m + 12.0 * w6.wi - 1.0);
assert!(s3.abs() < 1e-9, "N=3 α={alpha} κ={kappa}: ΣWᵐ − 1 = {s3:e}");
assert!(s6.abs() < 1e-9, "N=6 α={alpha} κ={kappa}: ΣWᵐ − 1 = {s6:e}");
}
}
#[test]
fn the_spread_is_alpha_times_the_unscaled_spread() {
for (alpha, beta, kappa) in SETS {
let p = UnscentedParams::new(alpha, beta, kappa);
for (n, w) in [(3.0, p.weights::<3>()), (6.0, p.weights::<6>())] {
let want = alpha * libm::sqrt(n + kappa);
assert!(
(w.gamma - want).abs() <= 1e-10 * want,
"N={n} α={alpha} κ={kappa}: γ = {}, want {want}",
w.gamma
);
}
}
}
#[test]
fn the_covariance_weight_differs_from_the_mean_weight_by_the_scaling_correction() {
let (alpha, kappa) = (0.9, 1.0);
let collapsed = UnscentedParams::new(alpha, alpha * alpha - 1.0, kappa).weights::<3>();
assert_eq!(
collapsed.w0c, collapsed.w0m,
"β = α² − 1 leaves no correction, so the two weights are the same number"
);
let base = UnscentedParams::new(alpha, 2.0, kappa).weights::<3>();
let shifted = UnscentedParams::new(alpha, 2.75, kappa).weights::<3>();
let slope = shifted.w0c - base.w0c;
assert!(
(slope - 0.75).abs() < 1e-14,
"β moved by 0.75 and W₀ᶜ by {slope}"
);
assert_eq!(shifted.w0m, base.w0m, "β must not reach the mean weight");
assert_eq!(shifted.gamma, base.gamma, "β must not reach the spread");
}
#[test]
fn the_transform_is_exact_for_an_affine_map() {
let mean = [1.0, -2.0, 0.5];
let s = [[2.0, 0.0, 0.0], [0.5, 1.5, 0.0], [-0.3, 0.4, 1.0]];
let a = [[1.0, 0.2, -0.1], [0.0, 1.3, 0.4], [0.5, -0.2, 0.9]];
let b = [0.7, -0.3, 1.1];
let params = UnscentedParams::new(1.0, 2.0, 0.0);
let (ybar, cov) = unscented_transform::<3, 3, 7>(¶ms, &mean, &s, |x| {
let mut y = b;
for i in 0..3 {
for j in 0..3 {
y[i] += a[i][j] * x[j];
}
}
y
});
let mut mean_ref = b;
for i in 0..3 {
for j in 0..3 {
mean_ref[i] += a[i][j] * mean[j];
}
}
let mut p = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
for k in 0..3 {
p[i][j] += s[i][k] * s[j][k];
}
}
}
let mut ap = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
for k in 0..3 {
ap[i][j] += a[i][k] * p[k][j];
}
}
}
let mut cov_ref = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
for k in 0..3 {
cov_ref[i][j] += ap[i][k] * a[j][k];
}
}
}
for i in 0..3 {
assert!((ybar[i] - mean_ref[i]).abs() < 1e-12, "mean[{i}]");
for j in 0..3 {
assert!(
(cov[i][j] - cov_ref[i][j]).abs() < 1e-11,
"cov[{i}][{j}]: {} vs {}",
cov[i][j],
cov_ref[i][j]
);
}
}
}
#[test]
fn the_residuals_are_taken_about_the_transformed_mean_not_the_center() {
let mean = [0.3, -0.7];
let s = [[1.1, 0.0], [-0.4, 0.9]];
let params = UnscentedParams::new(1.0, 2.0, 0.0);
let map = |x: &[f64; 2]| [x[0] * x[0] + x[1], libm::sin(x[0]) * x[1]];
let (ybar, cov) = unscented_transform::<2, 2, 5>(¶ms, &mean, &s, map);
let w = params.weights::<2>();
let pts = sigma_points::<2, 5>(&mean, &s, w.gamma);
let y: [[f64; 2]; 5] = core::array::from_fn(|i| map(&pts[i]));
let mut want_mean = [0.0; 2];
for j in 0..2 {
want_mean[j] = w.w0m * y[0][j];
for i in 1..5 {
want_mean[j] += w.wi * y[i][j];
}
}
let mut want_cov = [[0.0; 2]; 2];
for i in 0..5 {
let weight = if i == 0 { w.w0c } else { w.wi };
for a in 0..2 {
for b in 0..2 {
want_cov[a][b] += weight * (y[i][a] - want_mean[a]) * (y[i][b] - want_mean[b]);
}
}
}
for a in 0..2 {
assert!((ybar[a] - want_mean[a]).abs() < 1e-13, "mean[{a}]");
for b in 0..2 {
assert!(
(cov[a][b] - want_cov[a][b]).abs() < 1e-13,
"cov[{a}][{b}]: {} vs {}",
cov[a][b],
want_cov[a][b]
);
}
}
}
#[test]
fn the_transform_is_exact_for_an_affine_map_far_from_the_origin() {
let mean = [1e6, -2e6, 5e5];
let s = [[2e-3, 0.0, 0.0], [5e-4, 1.5e-3, 0.0], [-3e-4, 4e-4, 1e-3]];
let a = [[1.0, 0.2, -0.1], [0.0, 1.3, 0.4], [0.5, -0.2, 0.9]];
let params = UnscentedParams::gaussian();
let (_, cov) = unscented_transform::<3, 3, 7>(¶ms, &mean, &s, |x| {
let mut y = [0.0; 3];
for i in 0..3 {
for j in 0..3 {
y[i] += a[i][j] * x[j];
}
}
y
});
let mut asx = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
for k in 0..3 {
asx[i][j] += a[i][k] * s[k][j];
}
}
}
let mut want = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
for k in 0..3 {
want[i][j] += asx[i][k] * asx[j][k];
}
}
}
let mut worst: f64 = 0.0;
for i in 0..3 {
for j in 0..3 {
let scale = (want[i][i] * want[j][j]).sqrt();
worst = worst.max((cov[i][j] - want[i][j]).abs() / scale);
}
}
assert!(worst < 1e-2, "worst covariance departure {worst:e}");
}
#[test]
fn cholesky_factors_and_rejects() {
let p = [[4.0, 2.0, 0.6], [2.0, 3.0, 0.4], [0.6, 0.4, 1.0]];
let l = cholesky::<3>(&p).unwrap();
for i in 0..3 {
for j in 0..3 {
let mut acc = 0.0;
for k in 0..3 {
acc += l[i][k] * l[j][k];
}
assert!((acc - p[i][j]).abs() < 1e-12, "L Lᵀ [{i}][{j}]");
}
}
let bad = [[1.0, 2.0, 0.0], [2.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
assert!(cholesky::<3>(&bad).is_none());
}
#[test]
#[allow(clippy::too_many_lines)] fn the_nonlinear_covariance_matches_the_stt_and_gauss_hermite() {
use spacelab_propagation::driver::{Controls, integrate};
use spacelab_propagation::forces::{J2, Newtonian, PointMass};
use spacelab_propagation::stt::{
VariationalStt, augment_stt, augmented_len, map_covariance, map_deviation, split_stt,
};
use spacelab_propagation::{Dop853, Tolerance};
const MU: f64 = 3.986_004_418e14;
const RE: f64 = 6_378_137.0;
const J2_COEFF: f64 = 1.082_626_68e-3;
const M: usize = augmented_len(6);
let y0 = [6.9e6, 1.0e6, 0.8e6, -1.5e3, 6.8e3, 2.5e3];
let t1 = 1800.0;
let force = Newtonian((
PointMass { mu: MU },
J2 {
mu: MU,
re: RE,
j2: J2_COEFF,
},
));
let sys = VariationalStt::<6, _>::new(&force);
let aug0: [f64; M] = augment_stt::<6, M>(&y0);
let traj = integrate(
&Dop853,
&sys,
0.0,
&aug0,
t1,
10.0,
Controls::new(Tolerance::new(1e-12, 1e-12), 1e-6),
)
.unwrap();
let (_, phi, psi) = split_stt::<6, M>(&traj.final_state());
let g = |dx0: &[f64; 6]| map_deviation(&phi, &psi, dx0);
let sigma = [500.0, 500.0, 500.0, 0.5, 0.5, 0.5];
let mut p0 = [[0.0; 6]; 6];
for i in 0..6 {
p0[i][i] = sigma[i] * sigma[i];
}
let s0 = cholesky::<6>(&p0).unwrap();
let params = UnscentedParams::new(1.0, 2.0, 3.0 - 6.0);
let (ut_mean, ut_cov) = unscented_transform::<6, 6, 13>(¶ms, &[0.0; 6], &s0, g);
let (an_mean, an_cov) = map_covariance(&phi, &psi, &p0);
let nodes = [-(3.0_f64.sqrt()), 0.0, 3.0_f64.sqrt()];
let wts = [1.0 / 6.0, 2.0 / 3.0, 1.0 / 6.0];
let mut mean_gh = [0.0; 6];
let mut second = [[0.0; 6]; 6];
for gpt in 0..3usize.pow(6) {
let mut idx = gpt;
let mut w = 1.0;
let mut x = [0.0; 6];
for dim in 0..6 {
let t = idx % 3;
idx /= 3;
w *= wts[t];
x[dim] = sigma[dim] * nodes[t];
}
let y = g(&x);
for i in 0..6 {
mean_gh[i] += w * y[i];
for j in 0..6 {
second[i][j] += w * y[i] * y[j];
}
}
}
let mut cov_gh = [[0.0; 6]; 6];
for i in 0..6 {
for j in 0..6 {
cov_gh[i][j] = second[i][j] - mean_gh[i] * mean_gh[j];
}
}
let mut cov_scale = 0.0_f64;
let mut mean_scale = 0.0_f64;
for i in 0..6 {
cov_scale = nan_max(cov_scale, cov_gh[i][i].abs());
mean_scale = nan_max(mean_scale, mean_gh[i].abs());
}
let mut mean_err_gh = 0.0_f64;
let mut mean_err_an = 0.0_f64;
let mut cov_err_gh = 0.0_f64;
let mut cov_err_an = 0.0_f64;
for i in 0..6 {
mean_err_gh = nan_max(mean_err_gh, (ut_mean[i] - mean_gh[i]).abs());
mean_err_an = mean_err_an.max((ut_mean[i] - an_mean[i]).abs());
for j in 0..6 {
cov_err_gh = nan_max(cov_err_gh, (ut_cov[i][j] - cov_gh[i][j]).abs());
cov_err_an = cov_err_an.max((ut_cov[i][j] - an_cov[i][j]).abs());
}
}
println!(
"UT vs Gauss-Hermite: mean rel err {:.3e}, cov rel err {:.3e} (mean scale {mean_scale:.3e} m, cov scale {cov_scale:.3e})",
mean_err_gh / mean_scale,
cov_err_gh / cov_scale
);
println!(
"UT vs analytic STT (Park-Scheeres): mean rel err {:.3e}, cov rel err {:.3e}",
mean_err_an / mean_scale,
cov_err_an / cov_scale
);
assert!(
mean_scale > 0.0,
"the curvature must bias the mean off zero"
);
assert!(
mean_err_gh / mean_scale < 1e-9,
"UT mean must match Gauss-Hermite to rounding, got {:.3e}",
mean_err_gh / mean_scale
);
assert!(
cov_err_gh / cov_scale < 1e-7,
"UT covariance gap to Gauss-Hermite exceeds the expected kurtosis bound, got {:.3e}",
cov_err_gh / cov_scale
);
}
}