use crate::fim::{crlb, information_matrix, sym_eig};
use crate::lunar_datum::llr_row_datum7;
use crate::lunar_llr_geometry::{reflectors, stations};
const R_MOON_M: f64 = 1_737_400.0;
const K: [usize; 2] = [0, 3];
const M_IDX: [usize; 5] = [1, 2, 4, 5, 6];
#[derive(Debug, Clone)]
pub struct DatumIdentifiability {
pub info: Vec<Vec<f64>>,
pub n_obs: usize,
pub eigenvalues: Vec<f64>,
pub defect: usize,
pub origin_scale_corr: f64,
pub degeneracy_metric: f64,
pub origin_crlb_m: f64,
pub crlb_diag: Vec<f64>,
}
pub fn llr_datum_rows(
sigma_range_m: f64,
t0_jc: f64,
days: f64,
step_hours: f64,
) -> (Vec<[f64; 7]>, f64) {
const JD_J2000: f64 = 2_451_545.0;
let step_jc = step_hours / (24.0 * 36_525.0);
let n_steps = (days * 24.0 / step_hours).ceil() as usize + 1;
let refls = reflectors();
let stats = stations();
let mut rows: Vec<[f64; 7]> = Vec::new();
for step in 0..n_steps {
let t_tt_jc = t0_jc + step as f64 * step_jc;
let jd_tt = JD_J2000 + t_tt_jc * 36_525.0;
let jd_ut1 = jd_tt;
let r_moon = crate::ephem::moon_position(t_tt_jc);
for refl in &refls {
let r_refl = crate::lunar_llr_geometry::reflector_inertial(refl.pa_body_m, t_tt_jc);
let rrel = [
r_refl[0] - r_moon[0],
r_refl[1] - r_moon[1],
r_refl[2] - r_moon[2],
];
let earth_facing_dot =
rrel[0] * (-r_moon[0]) + rrel[1] * (-r_moon[1]) + rrel[2] * (-r_moon[2]);
if earth_facing_dot <= 0.0 {
continue;
}
let r_refl_ecef = crate::cio::gcrs_to_itrs(r_refl, jd_tt, jd_ut1, 0.0, 0.0);
for st in &stats {
let g = crate::frames::Geodetic {
lat_rad: st.lat_deg.to_radians(),
lon_rad: st.lon_deg.to_radians(),
alt_m: st.alt_m,
};
let el_rad = crate::frames::elevation(g, r_refl_ecef);
if el_rad <= 0.0 {
continue;
}
let row7 = llr_row_datum7(st, refl.pa_body_m, t_tt_jc, jd_ut1);
rows.push(row7);
}
}
}
(rows, sigma_range_m)
}
pub fn assemble_multi_info(blocks: &[(Vec<[f64; 7]>, f64)]) -> Vec<Vec<f64>> {
let mut combined = vec![vec![0.0_f64; 7]; 7];
for (rows, sigma) in blocks {
if rows.is_empty() {
continue;
}
let weight = 1.0 / (sigma * sigma);
let pre_rows: Vec<Vec<f64>> = rows
.iter()
.map(|r| {
let mut row = r.to_vec();
row[3..7].iter_mut().for_each(|v| *v /= R_MOON_M);
row
})
.collect();
let block_weights = vec![weight; pre_rows.len()];
let block_info = information_matrix(&pre_rows, &block_weights);
for (ci, bi) in combined.iter_mut().zip(block_info.iter()) {
for (cv, bv) in ci.iter_mut().zip(bi.iter()) {
*cv += bv;
}
}
}
combined
}
pub fn assemble_llr_info(
sigma_range_m: f64,
t0_jc: f64,
days: f64,
step_hours: f64,
) -> (Vec<Vec<f64>>, usize) {
let (rows, sigma) = llr_datum_rows(sigma_range_m, t0_jc, days, step_hours);
let n = rows.len();
(assemble_multi_info(&[(rows, sigma)]), n)
}
pub fn decompose(info: &[Vec<f64>], rel_tol: f64) -> DatumIdentifiability {
let cr = crlb(info, rel_tol);
let eigenvalues = cr.eigenvalues.clone();
let defect = cr.defect;
let crlb_diag = cr.crlb_std.clone();
let i_kk = [
[info[K[0]][K[0]], info[K[0]][K[1]]],
[info[K[1]][K[0]], info[K[1]][K[1]]],
];
let i_km: [[f64; 5]; 2] =
std::array::from_fn(|ki| std::array::from_fn(|mi| info[K[ki]][M_IDX[mi]]));
let i_mm: Vec<Vec<f64>> = M_IDX
.iter()
.map(|&r| M_IDX.iter().map(|&c| info[r][c]).collect::<Vec<f64>>())
.collect();
let d_inv = crlb(&i_mm, rel_tol).pseudo_covariance;
let b: [[f64; 5]; 2] = std::array::from_fn(|ki| {
std::array::from_fn(|j| {
i_km[ki]
.iter()
.zip(d_inv.iter())
.map(|(v, dl)| v * dl[j])
.sum::<f64>()
})
});
let c: [[f64; 2]; 2] = std::array::from_fn(|ki| {
std::array::from_fn(|kj| {
b[ki]
.iter()
.zip(i_km[kj].iter())
.map(|(bv, ikv)| bv * ikv)
.sum::<f64>()
})
});
let s = [
[i_kk[0][0] - c[0][0], i_kk[0][1] - c[0][1]],
[i_kk[1][0] - c[1][0], i_kk[1][1] - c[1][1]],
];
let s_mat = vec![vec![s[0][0], s[0][1]], vec![s[1][0], s[1][1]]];
let degeneracy_metric = sym_eig(&s_mat).values[0];
let det = s[0][0] * s[1][1] - s[0][1] * s[0][1];
let (origin_crlb_m, origin_scale_corr) = if det > 0.0 {
let s_inv_00 = s[1][1] / det; let s_inv_01 = -s[0][1] / det; let s_inv_11 = s[0][0] / det; let crlb_m = s_inv_00.max(0.0).sqrt();
let corr = if s_inv_00 > 0.0 && s_inv_11 > 0.0 {
s_inv_01 / (s_inv_00 * s_inv_11).sqrt()
} else {
0.0
};
(crlb_m, corr)
} else {
(f64::INFINITY, 0.0)
};
DatumIdentifiability {
info: info.to_vec(),
n_obs: 0,
eigenvalues,
defect,
origin_scale_corr,
degeneracy_metric,
origin_crlb_m,
crlb_diag,
}
}
pub fn llr_identifiability(
sigma_range_m: f64,
t0_jc: f64,
days: f64,
step_hours: f64,
) -> DatumIdentifiability {
let (info, n_obs) = assemble_llr_info(sigma_range_m, t0_jc, days, step_hours);
let mut d = decompose(&info, 1e-12);
d.n_obs = n_obs;
d
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn degeneracy_metric_equals_schur_min_eigenvalue_and_bounds_origin_crlb() {
let rho = 0.98_f64;
let mut info = vec![vec![0.0; 7]; 7];
for (i, row) in info.iter_mut().enumerate() {
row[i] = 1.0;
}
info[0][3] = rho;
info[3][0] = rho;
let d = decompose(&info, 1e-12);
assert!(
(d.degeneracy_metric - (1.0 - rho)).abs() < 1e-9,
"metric {} vs 1-rho {}",
d.degeneracy_metric,
1.0 - rho
);
let expected_crlb = (1.0 / (1.0 - rho * rho)).sqrt();
assert!(
(d.origin_crlb_m - expected_crlb).abs() < 1e-6 * expected_crlb,
"origin crlb {} vs {}",
d.origin_crlb_m,
expected_crlb
);
assert!(
(d.origin_scale_corr.abs() - rho).abs() < 1e-9,
"|corr| {} vs rho {}",
d.origin_scale_corr.abs(),
rho
);
}
#[test]
fn schur_path_equals_full_inverse_path_with_km_coupling() {
let rho = 0.6_f64;
let mut info = vec![vec![0.0; 7]; 7];
for (i, row) in info.iter_mut().enumerate() {
row[i] = 1.0;
}
info[0][3] = rho;
info[3][0] = rho;
info[0][1] = 0.3;
info[1][0] = 0.3;
let d = decompose(&info, 1e-12);
let full_pinv_00 = crate::fim::crlb(&info, 1e-12).pseudo_covariance[0][0];
assert!(
(d.origin_crlb_m - full_pinv_00.sqrt()).abs() < 1e-9,
"Schur path origin_crlb_m={} vs full-inverse sqrt({})={}",
d.origin_crlb_m,
full_pinv_00,
full_pinv_00.sqrt()
);
}
#[test]
fn llr_seven_param_shows_origin_scale_near_degeneracy() {
let t0_jc = (2_460_310.5 - 2_451_545.0) / 36_525.0; let d = llr_identifiability(0.003, t0_jc, 29.5, 6.0);
assert!(d.n_obs > 20, "schedule populated; got {}", d.n_obs);
assert!(
d.origin_scale_corr.abs() > 0.9 && d.origin_scale_corr.abs() < 0.9999,
"near-degeneracy expected (structural reproduction); got {}",
d.origin_scale_corr
);
assert!(
d.degeneracy_metric > 0.0 && d.degeneracy_metric.is_finite(),
"metric must be finite positive; got {}",
d.degeneracy_metric
);
assert_eq!(
d.defect, 0,
"real DE440 libration lifts the 7-param defect to 0; got {}",
d.defect
);
}
#[test]
fn adding_an_offradial_technique_collapses_the_origin_scale_degeneracy() {
use crate::lunar_datum::{orbiter_position, orbiter_range_row_datum7, vlbi_row_datum7};
use crate::lunar_llr_geometry::{reflector_inertial, stations};
let t0 = (2_460_310.5 - 2_451_545.0) / 36_525.0;
let (llr_rows, llr_sig) = llr_datum_rows(0.003, t0, 29.5, 6.0);
let beacon = [0.5_f64 * 1_737_400.0, 0.866 * 1_737_400.0, 0.0];
let st1 = stations()[1]; let st2 = stations()[0];
let step_jc = 6.0 / (24.0 * 36_525.0);
let mut vlbi_rows = Vec::new();
let mut orb_rows = Vec::new();
for k in 0..120 {
let t = t0 + k as f64 * step_jc;
let r_moon = crate::ephem::moon_position(t);
let r_b = reflector_inertial(beacon, t);
let earth_facing = (r_b[0] - r_moon[0]) * (-r_moon[0])
+ (r_b[1] - r_moon[1]) * (-r_moon[1])
+ (r_b[2] - r_moon[2]) * (-r_moon[2]);
if earth_facing <= 0.0 {
continue;
} let jd_ut1 = t * 36_525.0 + 2_451_545.0;
vlbi_rows.push(vlbi_row_datum7(&st1, &st2, beacon, t, jd_ut1));
let r_orb = orbiter_position(100.0, 88.0, 30.0, k as f64 * 13.0, t0, t);
orb_rows.push(orbiter_range_row_datum7(r_orb, beacon, t));
}
assert!(vlbi_rows.len() > 20 && orb_rows.len() > 20);
let llr_only = decompose(&assemble_multi_info(&[(llr_rows.clone(), llr_sig)]), 1e-12);
let with_vlbi = decompose(
&assemble_multi_info(&[
(llr_rows.clone(), llr_sig),
(vlbi_rows, 1e-11),
]),
1e-12,
);
let with_orb = decompose(
&assemble_multi_info(&[
(llr_rows, llr_sig),
(orb_rows, 0.05),
]),
1e-12,
);
assert!(
with_vlbi.degeneracy_metric > llr_only.degeneracy_metric,
"VLBI must raise the metric: {} -> {}",
llr_only.degeneracy_metric,
with_vlbi.degeneracy_metric
);
assert!(
with_vlbi.origin_crlb_m < llr_only.origin_crlb_m,
"VLBI must shrink origin CRLB: {} -> {}",
llr_only.origin_crlb_m,
with_vlbi.origin_crlb_m
);
assert!(
with_vlbi.origin_scale_corr.abs() < llr_only.origin_scale_corr.abs(),
"VLBI must reduce |corr|: {} -> {}",
llr_only.origin_scale_corr,
with_vlbi.origin_scale_corr
);
let frac =
|k: usize| (llr_only.crlb_diag[k] - with_vlbi.crlb_diag[k]) / llr_only.crlb_diag[k];
assert!(
frac(1) > frac(0),
"VLBI must improve transverse t_y more than radial t_x: frac_ty={} frac_tx={}",
frac(1),
frac(0)
);
assert!(
with_orb.degeneracy_metric > llr_only.degeneracy_metric,
"orbiter must raise the metric: {} -> {}",
llr_only.degeneracy_metric,
with_orb.degeneracy_metric
);
assert!(
with_orb.origin_crlb_m < llr_only.origin_crlb_m,
"orbiter must shrink origin CRLB: {} -> {}",
llr_only.origin_crlb_m,
with_orb.origin_crlb_m
);
}
}