diff --git a/assets/dashboard.html b/assets/dashboard.html new file mode 100644 index 0000000..9956af4 --- /dev/null +++ b/assets/dashboard.html @@ -0,0 +1,550 @@ + + + + + + Sattime Cyber-Physical HUD + + + + + +
+

SATTIME OMNI-SENSOR HUD

+
+
+ DISCONNECTED +
+
+ +
+ +
+
Polar Satellite Skyplot
+
+ +
+
+
+
Receiver X/Y/Z
+
0, 0, 0
+
+
+
Receiver Vx/Vy/Vz
+
0, 0, 0
+
+
+
Clock Bias
+
0.000000 s
+
+
+
Clock Drift
+
0.000 ppm
+
+
+
+ + +
+
+
Real-Time EKF residuals (S-Curves)
+
+ +
+
+ +
+
Active Tracking Channels
+
+ + + + + + + + + + + + + + + + + + + +
ChSatelliteStatusF_ExpectedOffsetSNRTECS4σ_φ
No active channels. Waiting for telemetry data...
+
+
+
+
+ + + + diff --git a/src/daemon.rs b/src/daemon.rs index 65aac67..bbe4402 100644 --- a/src/daemon.rs +++ b/src/daemon.rs @@ -139,11 +139,11 @@ fn write_to_ntp_shm(shm_unit: usize, target_adjustment: f64) -> Result<(), Strin unsafe { std::ptr::write_volatile(&mut (*shm_ptr).mode, 1); std::ptr::write_volatile(&mut (*shm_ptr).valid, 0); - std::sync::atomic::compiler_fence(std::sync::atomic::Ordering::SeqCst); + std::sync::atomic::fence(std::sync::atomic::Ordering::SeqCst); let count = std::ptr::read_volatile(&(*shm_ptr).count); std::ptr::write_volatile(&mut (*shm_ptr).count, count.wrapping_add(1)); - std::sync::atomic::compiler_fence(std::sync::atomic::Ordering::SeqCst); + std::sync::atomic::fence(std::sync::atomic::Ordering::SeqCst); std::ptr::write_volatile(&mut (*shm_ptr).clock_time_stamp_sec, clock_sec); std::ptr::write_volatile(&mut (*shm_ptr).clock_time_stamp_usec, clock_usec); @@ -154,10 +154,10 @@ fn write_to_ntp_shm(shm_unit: usize, target_adjustment: f64) -> Result<(), Strin std::ptr::write_volatile(&mut (*shm_ptr).leap, 0); std::ptr::write_volatile(&mut (*shm_ptr).precision, -20); std::ptr::write_volatile(&mut (*shm_ptr).nsamples, 1); - std::sync::atomic::compiler_fence(std::sync::atomic::Ordering::SeqCst); + std::sync::atomic::fence(std::sync::atomic::Ordering::SeqCst); std::ptr::write_volatile(&mut (*shm_ptr).count, count.wrapping_add(2)); - std::sync::atomic::compiler_fence(std::sync::atomic::Ordering::SeqCst); + std::sync::atomic::fence(std::sync::atomic::Ordering::SeqCst); std::ptr::write_volatile(&mut (*shm_ptr).valid, 1); } @@ -444,16 +444,21 @@ pub struct CompletedPassData { } pub struct ConsensusSteeringEngine { - pub passes: Vec, + pub passes: std::collections::VecDeque, } impl ConsensusSteeringEngine { pub fn new() -> Self { - Self { passes: Vec::new() } + Self { + passes: std::collections::VecDeque::new(), + } } pub fn add_pass_result(&mut self, pass: CompletedPassData) { - self.passes.push(pass); + self.passes.push_back(pass); + if self.passes.len() > 50 { + self.passes.pop_front(); + } } pub fn get_consensus_update(&self) -> Option<(f64, f64)> { @@ -480,7 +485,9 @@ impl ConsensusSteeringEngine { if weight > 0.0 { total_weight += weight; - weighted_offset += pass.offset_seconds * weight; + let elapsed = (chrono::Utc::now() - pass.timestamp).num_seconds().max(0) as f64; + let extrapolated_offset = pass.offset_seconds + (pass.freq_drift_ppm * 1e-6 * elapsed); + weighted_offset += extrapolated_offset * weight; weighted_drift += pass.freq_drift_ppm * weight; } } diff --git a/src/dsp.rs b/src/dsp.rs index 99e27d8..07de602 100644 --- a/src/dsp.rs +++ b/src/dsp.rs @@ -3,6 +3,18 @@ use chrono::{DateTime, Utc}; use num_complex::Complex; use rustfft::FftPlanner; use std::collections::VecDeque; +use std::sync::LazyLock; + +#[cfg(any(target_arch = "x86", target_arch = "x86_64"))] +static HAS_AVX2_FMA: LazyLock = LazyLock::new(|| { + std::is_x86_feature_detected!("avx2") && std::is_x86_feature_detected!("fma") +}); + +#[cfg(any(target_arch = "arm", target_arch = "aarch64"))] +static HAS_NEON: LazyLock = LazyLock::new(|| { + std::arch::is_aarch64_feature_detected!("neon") +}); + #[derive(clap::ValueEnum, Debug, Clone, Copy, PartialEq, Eq, Default)] pub enum Modulation { #[default] @@ -331,13 +343,54 @@ impl FirDecimator { Complex::new(re, im) } - pub fn process(&mut self, input: &[Complex], output: &mut Vec>) { - if self.decimation_factor <= 1 { - output.reserve(input.len()); - output.extend_from_slice(input); - return; + #[cfg(target_arch = "x86_64")] + #[target_feature(enable = "avx2,fma")] + pub unsafe fn process_avx2(&mut self, input: &[Complex], output: &mut Vec>) { + let num_taps = self.taps.len(); + let hist_len = self.history.len(); + let total_len = hist_len + input.len(); + + let projected_capacity = (input.len() / self.decimation_factor) + 2; + output.reserve(projected_capacity); + + let mut idx = self.pending_offset; + while idx < hist_len && idx + num_taps <= total_len { + let mut sum = Complex::new(0.0f32, 0.0f32); + for n in 0..num_taps { + let sample_idx = idx + n; + let sample = if sample_idx < hist_len { + self.history[sample_idx] + } else { + input[sample_idx - hist_len] + }; + sum += sample * self.taps[n]; + } + output.push(sum); + idx += self.decimation_factor; + } + + while idx + num_taps <= total_len { + let input_offset = idx - hist_len; + let input_slice = &input[input_offset..input_offset + num_taps]; + output.push(unsafe { self.compute_x86_64(input_slice) }); + idx += self.decimation_factor; + } + + self.pending_offset = idx.saturating_sub(input.len()); + + if input.len() >= hist_len { + self.history + .copy_from_slice(&input[input.len() - hist_len..]); + } else { + let shift = hist_len - input.len(); + self.history.copy_within(input.len().., 0); + self.history[shift..].copy_from_slice(input); } + } + #[cfg(target_arch = "aarch64")] + #[target_feature(enable = "neon")] + pub unsafe fn process_neon(&mut self, input: &[Complex], output: &mut Vec>) { let num_taps = self.taps.len(); let hist_len = self.history.len(); let total_len = hist_len + input.len(); @@ -346,7 +399,6 @@ impl FirDecimator { output.reserve(projected_capacity); let mut idx = self.pending_offset; - // Boundary Phase: process samples that overlap with history while idx < hist_len && idx + num_taps <= total_len { let mut sum = Complex::new(0.0f32, 0.0f32); for n in 0..num_taps { @@ -362,11 +414,10 @@ impl FirDecimator { idx += self.decimation_factor; } - // Main Phase: contiguous slice processing (using SIMD) while idx + num_taps <= total_len { let input_offset = idx - hist_len; let input_slice = &input[input_offset..input_offset + num_taps]; - output.push(self.compute(input_slice)); + output.push(unsafe { self.compute_aarch64(input_slice) }); idx += self.decimation_factor; } @@ -381,6 +432,71 @@ impl FirDecimator { self.history[shift..].copy_from_slice(input); } } + + pub fn process_scalar(&mut self, input: &[Complex], output: &mut Vec>) { + let num_taps = self.taps.len(); + let hist_len = self.history.len(); + let total_len = hist_len + input.len(); + + let projected_capacity = (input.len() / self.decimation_factor) + 2; + output.reserve(projected_capacity); + + let mut idx = self.pending_offset; + while idx < hist_len && idx + num_taps <= total_len { + let mut sum = Complex::new(0.0f32, 0.0f32); + for n in 0..num_taps { + let sample_idx = idx + n; + let sample = if sample_idx < hist_len { + self.history[sample_idx] + } else { + input[sample_idx - hist_len] + }; + sum += sample * self.taps[n]; + } + output.push(sum); + idx += self.decimation_factor; + } + + while idx + num_taps <= total_len { + let input_offset = idx - hist_len; + let input_slice = &input[input_offset..input_offset + num_taps]; + output.push(self.compute_scalar(input_slice)); + idx += self.decimation_factor; + } + + self.pending_offset = idx.saturating_sub(input.len()); + + if input.len() >= hist_len { + self.history + .copy_from_slice(&input[input.len() - hist_len..]); + } else { + let shift = hist_len - input.len(); + self.history.copy_within(input.len().., 0); + self.history[shift..].copy_from_slice(input); + } + } + + pub fn process(&mut self, input: &[Complex], output: &mut Vec>) { + if self.decimation_factor <= 1 { + output.reserve(input.len()); + output.extend_from_slice(input); + return; + } + + #[cfg(target_arch = "x86_64")] + if *HAS_AVX2_FMA { + unsafe { self.process_avx2(input, output) }; + return; + } + + #[cfg(target_arch = "aarch64")] + if *HAS_NEON { + unsafe { self.process_neon(input, output) }; + return; + } + + self.process_scalar(input, output); + } } fn complex_cholesky_6x6(a: &[[Complex; 6]; 6]) -> Option<[[Complex; 6]; 6]> { @@ -427,6 +543,67 @@ fn complex_cholesky_solve_6(a: &[[Complex; 6]; 6], b: &[Complex; 6]) - Some(x) } +/// The CLEAN Algorithm (Deconvolution / Orthogonal Matching Pursuit) +/// Iteratively subtracts 2D point-spread functions (PSF) to expose weak targets/signals. +pub fn clean_ambiguity_map( + map: &mut [Vec], + iterations: usize, + loop_gain: f32, +) -> Vec<(usize, usize, f32)> { + let n_rows = map.len(); + if n_rows == 0 { + return vec![]; + } + let n_cols = map[0].len(); + if n_cols == 0 { + return vec![]; + } + + let mut clean_components = Vec::new(); + let sigma_row = 2.0f32; + let sigma_col = 2.0f32; + + for _ in 0..iterations { + // 1. Locate absolute peak + let mut max_val = 0.0f32; + let mut peak_r = 0; + let mut peak_c = 0; + + for r in 0..n_rows { + for c in 0..n_cols { + let val = map[r][c].abs(); + if val > max_val { + max_val = val; + peak_r = r; + peak_c = c; + } + } + } + + // Check if peak is too small (e.g. down to noise floor) + if max_val < 1e-4 { + break; + } + + let peak_val = map[peak_r][peak_c]; + clean_components.push((peak_r, peak_c, peak_val)); + + // 2. Subtract ideal 2D Gaussian point-spread function + for r in 0..n_rows { + let dr = (r as f32 - peak_r as f32).powi(2); + for c in 0..n_cols { + let dc = (c as f32 - peak_c as f32).powi(2); + + // Ideal 2D Gaussian ambiguity lobe + let psf = (-dr / (2.0 * sigma_row.powi(2)) - dc / (2.0 * sigma_col.powi(2))).exp(); + map[r][c] -= loop_gain * peak_val * psf; + } + } + } + + clean_components +} + /// Extensive Cancellation Algorithm (ECA) filter /// Uses exact Orthogonal Subspace Projection on blocks of samples to obliterate /// the direct path and stationary clutter down to the noise floor. @@ -856,6 +1033,25 @@ impl DigitalDownConverter { f_shift: f64, sample_rate: f64, output: &mut [Complex], + ) { + #[cfg(any(target_arch = "x86", target_arch = "x86_64"))] + { + if is_x86_feature_detected!("avx2") && is_x86_feature_detected!("fma") { + unsafe { + self.process_avx2(input, f_shift, sample_rate, output); + return; + } + } + } + self.process_scalar(input, f_shift, sample_rate, output); + } + + fn process_scalar( + &mut self, + input: &[Complex], + f_shift: f64, + sample_rate: f64, + output: &mut [Complex], ) { if f_shift.abs() < 1e-6 { output.copy_from_slice(input); @@ -863,7 +1059,6 @@ impl DigitalDownConverter { } let phase_step = -2.0 * std::f64::consts::PI * f_shift / sample_rate; - // Use f64 phasor accumulation to prevent magnitude drift from repeated f32 multiplication let mut phasor_re = self.phase_acc.cos(); let mut phasor_im = self.phase_acc.sin(); let step_re = phase_step.cos(); @@ -879,7 +1074,6 @@ impl DigitalDownConverter { phasor_re = new_re; phasor_im = new_im; - // Renormalize every 256 samples to bound accumulated f64 drift if n % 256 == 0 { let mag = (phasor_re * phasor_re + phasor_im * phasor_im).sqrt(); if mag > 1e-15 { @@ -891,6 +1085,114 @@ impl DigitalDownConverter { self.phase_acc = phasor_im.atan2(phasor_re); } + + #[cfg(any(target_arch = "x86", target_arch = "x86_64"))] + #[target_feature(enable = "avx2,fma")] + unsafe fn process_avx2( + &mut self, + input: &[Complex], + f_shift: f64, + sample_rate: f64, + output: &mut [Complex], + ) { + use std::arch::x86_64::*; + + if f_shift.abs() < 1e-6 { + output.copy_from_slice(input); + return; + } + + let phase_step = -2.0 * std::f64::consts::PI * f_shift / sample_rate; + + let mut w_re = [0.0f32; 8]; + let mut w_im = [0.0f32; 8]; + for k in 0..8 { + let angle = (k as f64) * phase_step; + w_re[k] = angle.cos() as f32; + w_im[k] = angle.sin() as f32; + } + + let step_8_re = (8.0 * phase_step).cos(); + let step_8_im = (8.0 * phase_step).sin(); + + let mut phasor_re = self.phase_acc.cos(); + let mut phasor_im = self.phase_acc.sin(); + + let chunks = input.len() / 8; + let rem = input.len() % 8; + + let sign_mask = _mm256_set_ps(1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0); + + for c in 0..chunks { + let offset = c * 8; + + let mut p_re = [0.0f32; 8]; + let mut p_im = [0.0f32; 8]; + for k in 0..8 { + p_re[k] = (phasor_re * w_re[k] as f64 - phasor_im * w_im[k] as f64) as f32; + p_im[k] = (phasor_re * w_im[k] as f64 + phasor_im * w_re[k] as f64) as f32; + } + + let p_re_low = _mm256_set_ps( + p_re[3], p_re[3], p_re[2], p_re[2], p_re[1], p_re[1], p_re[0], p_re[0] + ); + let p_im_low = _mm256_set_ps( + p_im[3], p_im[3], p_im[2], p_im[2], p_im[1], p_im[1], p_im[0], p_im[0] + ); + let p_re_high = _mm256_set_ps( + p_re[7], p_re[7], p_re[6], p_re[6], p_re[5], p_re[5], p_re[4], p_re[4] + ); + let p_im_high = _mm256_set_ps( + p_im[7], p_im[7], p_im[6], p_im[6], p_im[5], p_im[5], p_im[4], p_im[4] + ); + + let in_ptr = input.as_ptr().add(offset) as *const f32; + let in_low = _mm256_loadu_ps(in_ptr); + let in_high = _mm256_loadu_ps(in_ptr.add(8)); + + let in_low_swapped = _mm256_shuffle_ps(in_low, in_low, 0xB1); + let in_low_swapped_signed = _mm256_mul_ps(in_low_swapped, sign_mask); + let out_low = _mm256_fmadd_ps(in_low, p_re_low, _mm256_mul_ps(in_low_swapped_signed, p_im_low)); + + let in_high_swapped = _mm256_shuffle_ps(in_high, in_high, 0xB1); + let in_high_swapped_signed = _mm256_mul_ps(in_high_swapped, sign_mask); + let out_high = _mm256_fmadd_ps(in_high, p_re_high, _mm256_mul_ps(in_high_swapped_signed, p_im_high)); + + let out_ptr = output.as_mut_ptr().add(offset) as *mut f32; + _mm256_storeu_ps(out_ptr, out_low); + _mm256_storeu_ps(out_ptr.add(8), out_high); + + let new_re = phasor_re * step_8_re - phasor_im * step_8_im; + let new_im = phasor_re * step_8_im + phasor_im * step_8_re; + phasor_re = new_re; + phasor_im = new_im; + + if c % 32 == 0 { + let mag = (phasor_re * phasor_re + phasor_im * phasor_im).sqrt(); + if mag > 1e-15 { + phasor_re /= mag; + phasor_im /= mag; + } + } + } + + let mut offset = chunks * 8; + let step_re = phase_step.cos(); + let step_im = phase_step.sin(); + for _ in 0..rem { + output[offset] = Complex::new( + (input[offset].re as f64 * phasor_re - input[offset].im as f64 * phasor_im) as f32, + (input[offset].re as f64 * phasor_im + input[offset].im as f64 * phasor_re) as f32, + ); + let new_re = phasor_re * step_re - phasor_im * step_im; + let new_im = phasor_re * step_im + phasor_im * step_re; + phasor_re = new_re; + phasor_im = new_im; + offset += 1; + } + + self.phase_acc = phasor_im.atan2(phasor_re); + } } #[derive(Debug, Clone)] @@ -962,6 +1264,11 @@ pub struct DemodChannel { pub eca_canceler: EcaCanceler, pub is_dual: bool, pub current_tec: f64, + pub raw_norms: Vec, + pub raw_norms2: Vec, + pub amp_history: VecDeque, + pub last_carrier_freq_offset: Option, + pub smoothed_free_freq: Option, } impl DemodChannel { @@ -1074,6 +1381,11 @@ impl DemodChannel { eca_canceler: EcaCanceler::new(), is_dual: false, current_tec: 0.0, + raw_norms: Vec::new(), + raw_norms2: Vec::new(), + amp_history: VecDeque::new(), + last_carrier_freq_offset: None, + smoothed_free_freq: None, } } @@ -1145,7 +1457,23 @@ impl DemodChannel { } } + let freq_to_use = if self.nominal_freq == 0.0 { + self.target_freq + } else { + self.nominal_freq + }; + let f_shift = freq_to_use - center_freq; + let dt_block = raw_iq.len() as f64; + if has_nan { + let phase_step = -2.0 * std::f64::consts::PI * f_shift / self.sample_rate; + self.ddc.phase_acc = (self.ddc.phase_acc + phase_step * dt_block).rem_euclid(2.0 * std::f64::consts::PI); + if self.is_dual { + let f_shift2 = self.target_freq2 - center_freq; + let phase_step2 = -2.0 * std::f64::consts::PI * f_shift2 / self.sample_rate; + self.ddc2.phase_acc = (self.ddc2.phase_acc + phase_step2 * dt_block).rem_euclid(2.0 * std::f64::consts::PI); + } + self.pll_tracker.is_locked = false; self.is_locked = false; self.symbol_locked = false; @@ -1157,6 +1485,14 @@ impl DemodChannel { // Guard clipping BEFORE DDC to prevent advancing phase_acc on discarded blocks if has_clipping { + let phase_step = -2.0 * std::f64::consts::PI * f_shift / self.sample_rate; + self.ddc.phase_acc = (self.ddc.phase_acc + phase_step * dt_block).rem_euclid(2.0 * std::f64::consts::PI); + if self.is_dual { + let f_shift2 = self.target_freq2 - center_freq; + let phase_step2 = -2.0 * std::f64::consts::PI * f_shift2 / self.sample_rate; + self.ddc2.phase_acc = (self.ddc2.phase_acc + phase_step2 * dt_block).rem_euclid(2.0 * std::f64::consts::PI); + } + self.pll_tracker.is_locked = false; self.is_locked = false; self.symbol_locked = false; @@ -1166,12 +1502,6 @@ impl DemodChannel { return; } - let freq_to_use = if self.nominal_freq == 0.0 { - self.target_freq - } else { - self.nominal_freq - }; - let f_shift = freq_to_use - center_freq; self.mixed_samples .resize(raw_iq.len(), Complex::new(0.0, 0.0)); @@ -1280,7 +1610,24 @@ impl DemodChannel { } } - // d. Perform Bussgang Normalization (Constant Modulus Projection) on narrowband decimated signal. + // d. Save raw norms for EKF adaptive fading before Bussgang Normalization + self.raw_norms.resize(self.decimated_samples.len(), 0.0); + for (i, s) in self.decimated_samples.iter().enumerate() { + let norm = s.norm() as f64; + self.raw_norms[i] = norm; + self.amp_history.push_back(norm); + if self.amp_history.len() > 1024 { + self.amp_history.pop_front(); + } + } + if self.is_dual { + self.raw_norms2.resize(self.decimated_samples2.len(), 0.0); + for (i, s) in self.decimated_samples2.iter().enumerate() { + self.raw_norms2[i] = s.norm() as f64; + } + } + + // Perform Bussgang Normalization (Constant Modulus Projection) on narrowband decimated signal. // NOTE: This must happen AFTER SNR estimation (which needs amplitude variance) // and AFTER ESPRIT bootstrap (which needs spectral amplitude structure), // but BEFORE the EKF tracking loop (which benefits from constant-modulus input). @@ -1306,6 +1653,10 @@ impl DemodChannel { // Execute single or multi-hypothesis tracking updates if let Some(ref mut bank) = self.tracking_bank { + // Dynamically update max_fade_steps to match the actual block/step size processed + if !raw_iq.is_empty() { + bank.max_fade_steps = (self.fade_timeout * self.sample_rate / raw_iq.len() as f64) as usize; + } let mut symbols = Vec::with_capacity(4); // Pre-allocate outside hot loop for (s_idx, &s) in self.decimated_samples.iter().enumerate() { for i in 0..3 { @@ -1318,6 +1669,7 @@ impl DemodChannel { let prev_ts = tracker.ts; tracker.ts = decimated_ts; tracker.predict(); + tracker.raw_amp = [self.raw_norms[s_idx], self.raw_norms2[s_idx]]; tracker.update_dual(s, s2); tracker.ts = prev_ts; } else if self.modulation != Modulation::Carrier && (tracker.ts - decimated_ts).abs() > 1e-9 { @@ -1348,6 +1700,7 @@ impl DemodChannel { let prev_ts = tracker.ts; tracker.ts = 1.0 / self.symbol_rate; // Symbol rate tracking ts scale tracker.predict(); + tracker.raw_amp = [self.raw_norms[s_idx], 0.0]; tracker.update(sym_raw); tracker.ts = prev_ts; } @@ -1355,6 +1708,7 @@ impl DemodChannel { let prev_ts = tracker.ts; tracker.ts = decimated_ts; tracker.predict(); + tracker.raw_amp = [self.raw_norms[s_idx], 0.0]; tracker.update(s); tracker.ts = prev_ts; } @@ -1430,8 +1784,8 @@ impl DemodChannel { let f_free = AppletonHartreeDispersion::cancel(self.nominal_freq, self.frequency2, f1_abs, f2_abs); self.frequency = f_free; - let theta1 = active_tracker.x[0]; - let theta2 = active_tracker.x[3]; + let theta1 = active_tracker.unwrapped_phase1; + let theta2 = active_tracker.unwrapped_phase2; if f1 != 0.0 && f2 != 0.0 && (f1 - f2).abs() > 1e-6 { let f1_sq = f1 * f1; let f2_sq = f2 * f2; @@ -1472,10 +1826,21 @@ impl DemodChannel { let f1_abs = f1 + fd1; let f2_abs = f2 + fd2; let f_free = AppletonHartreeDispersion::cancel(self.nominal_freq, self.frequency2, f1_abs, f2_abs); - self.frequency = f_free; + + let smoothed = match (self.last_carrier_freq_offset, self.smoothed_free_freq) { + (Some(last_c), Some(last_s)) => { + let df_c = fd1 - last_c; + let m = 100.0; + (1.0 / m) * f_free + ((m - 1.0) / m) * (last_s + df_c) + } + _ => f_free, + }; + self.last_carrier_freq_offset = Some(fd1); + self.smoothed_free_freq = Some(smoothed); + self.frequency = smoothed; - let theta1 = tracker.x[0]; - let theta2 = tracker.x[3]; + let theta1 = tracker.unwrapped_phase1; + let theta2 = tracker.unwrapped_phase2; if f1 != 0.0 && f2 != 0.0 && (f1 - f2).abs() > 1e-6 { let f1_sq = f1 * f1; let f2_sq = f2 * f2; @@ -1507,6 +1872,7 @@ impl DemodChannel { let prev_ts = self.pll_tracker.ts; self.pll_tracker.ts = decimated_ts; self.pll_tracker.predict(); + self.pll_tracker.raw_amp = [self.raw_norms[s_idx], self.raw_norms2[s_idx]]; self.pll_tracker.update_dual(s, s2); self.pll_tracker.ts = prev_ts; } else if self.modulation != Modulation::Carrier && (self.pll_tracker.ts - decimated_ts).abs() > 1e-9 { @@ -1524,7 +1890,7 @@ impl DemodChannel { let dt_sample = decimated_ts; let true_theta = theta - (self.pll_tracker.x[1] / scale) * (3.0 - mu as f64) * dt_sample; - let cos_inv = true_theta.cos(); + let cos_inv = true_theta.cos(); let sin_inv = true_theta.sin(); let sym_raw = Complex::new( (sym_derot.re as f64 * cos_inv - sym_derot.im as f64 * sin_inv) @@ -1535,6 +1901,7 @@ impl DemodChannel { let prev_ts = self.pll_tracker.ts; self.pll_tracker.ts = 1.0 / self.symbol_rate; self.pll_tracker.predict(); + self.pll_tracker.raw_amp = [self.raw_norms[s_idx], 0.0]; self.pll_tracker.update(sym_raw); self.pll_tracker.ts = prev_ts; } @@ -1542,6 +1909,7 @@ impl DemodChannel { let prev_ts = self.pll_tracker.ts; self.pll_tracker.ts = decimated_ts; self.pll_tracker.predict(); + self.pll_tracker.raw_amp = [self.raw_norms[s_idx], 0.0]; self.pll_tracker.update(s); self.pll_tracker.ts = prev_ts; } @@ -1563,10 +1931,21 @@ impl DemodChannel { let f1_abs = f1 + fd1; let f2_abs = f2 + fd2; let f_free = AppletonHartreeDispersion::cancel(self.nominal_freq, self.frequency2, f1_abs, f2_abs); - self.frequency = f_free; - - let theta1 = self.pll_tracker.x[0]; - let theta2 = self.pll_tracker.x[3]; + + let smoothed = match (self.last_carrier_freq_offset, self.smoothed_free_freq) { + (Some(last_c), Some(last_s)) => { + let df_c = fd1 - last_c; + let m = 100.0; + (1.0 / m) * f_free + ((m - 1.0) / m) * (last_s + df_c) + } + _ => f_free, + }; + self.last_carrier_freq_offset = Some(fd1); + self.smoothed_free_freq = Some(smoothed); + self.frequency = smoothed; + + let theta1 = self.pll_tracker.unwrapped_phase1; + let theta2 = self.pll_tracker.unwrapped_phase2; if f1 != 0.0 && f2 != 0.0 && (f1 - f2).abs() > 1e-6 { let f1_sq = f1 * f1; let f2_sq = f2 * f2; @@ -1657,6 +2036,8 @@ impl DemodChannel { self.decimated_samples.clear(); self.last_processed_len = 0; self.sample_count = 0; + self.last_carrier_freq_offset = None; + self.smoothed_free_freq = None; } } diff --git a/src/ekf.rs b/src/ekf.rs index 396e41f..5fee45d 100644 --- a/src/ekf.rs +++ b/src/ekf.rs @@ -91,6 +91,11 @@ pub struct CarrierPllEkf { pub pr_sum_q_sq: f64, pub convergence_guard: usize, // Samples remaining before lock_metric can trigger unlock pub frequency_ratio: f64, + pub raw_amp: [f64; 2], + pub unwrapped_phase1: f64, + pub unwrapped_phase2: f64, + pub last_innovation: f64, + pub innovation_history: VecDeque, } impl CarrierPllEkf { @@ -122,6 +127,11 @@ impl CarrierPllEkf { pr_sum_q_sq: 0.0, convergence_guard: 0, frequency_ratio: 1.0, + raw_amp: [0.0, 0.0], + unwrapped_phase1: 0.0, + unwrapped_phase2: 0.0, + last_innovation: 0.0, + innovation_history: VecDeque::new(), } } @@ -156,12 +166,23 @@ impl CarrierPllEkf { self.pr_sum_q_sq = 0.0; // Grace period: suppress unlock checks for 2048 samples so EKF can converge self.convergence_guard = 2048; + self.raw_amp = [0.0, 0.0]; + self.unwrapped_phase1 = initial_phase; + self.unwrapped_phase2 = initial_phase; + self.last_innovation = 0.0; + self.innovation_history.clear(); } pub fn predict(&mut self) { let dt = self.ts; let dt2 = 0.5 * dt * dt; + // Accumulate unwrapped phase changes + let dp1 = self.x[1] * dt + 0.5 * self.x[2] * dt * dt; + let dp2 = self.x[4] * dt + 0.5 * self.x[5] * dt * dt; + self.unwrapped_phase1 += dp1; + self.unwrapped_phase2 += dp2; + let mut f = Matrix6::identity(); f[(0, 1)] = dt; f[(0, 2)] = dt2; @@ -221,7 +242,11 @@ impl CarrierPllEkf { let derotated_re = sample_to_use.re as f64 * cos_theta - sample_to_use.im as f64 * sin_theta; let derotated_im = sample_to_use.re as f64 * sin_theta + sample_to_use.im as f64 * cos_theta; - let amp = (sample_to_use.re as f64).hypot(sample_to_use.im as f64); + let amp = if self.raw_amp[chan] > 0.0 { + self.raw_amp[chan] + } else { + (sample_to_use.re as f64).hypot(sample_to_use.im as f64) + }; let alpha = 0.005; self.envelope_ema = (1.0 - alpha) * self.envelope_ema + alpha * amp; @@ -293,6 +318,12 @@ impl CarrierPllEkf { if self.pr_sum_q_sq < 0.0 { self.pr_sum_q_sq = 0.0; } + + self.last_innovation = z; + self.innovation_history.push_back(z); + if self.innovation_history.len() > 2048 { + self.innovation_history.pop_front(); + } } let s_val = self.p[(idx, idx)] + r_effective; @@ -304,6 +335,13 @@ impl CarrierPllEkf { for r in 0..6 { self.x[r] += k[r] * z; } + // Accumulate unwrapped phase correction + let delta_phase = k[idx] * z; + if chan == 0 { + self.unwrapped_phase1 += delta_phase; + } else { + self.unwrapped_phase2 += delta_phase; + } self.x[idx] = (self.x[idx] + half_limit).rem_euclid(limit) - half_limit; if self.x.iter().any(|v| v.is_nan()) { @@ -359,8 +397,10 @@ impl CarrierPllEkf { pub fn update(&mut self, sample: Complex) { let Some((norm_re, pr)) = self.update_channel(0, sample) else { + self.raw_amp = [0.0, 0.0]; return; }; + self.raw_amp = [0.0, 0.0]; let beta = 0.001; self.lock_metric = (1.0 - beta) * self.lock_metric + beta * norm_re; @@ -382,11 +422,14 @@ impl CarrierPllEkf { pub fn update_dual(&mut self, sample1: Complex, sample2: Complex) { let Some((norm_re, pr)) = self.update_channel(0, sample1) else { + self.raw_amp = [0.0, 0.0]; return; }; if self.update_channel(1, sample2).is_none() { + self.raw_amp = [0.0, 0.0]; return; } + self.raw_amp = [0.0, 0.0]; let beta = 0.001; self.lock_metric = (1.0 - beta) * self.lock_metric + beta * norm_re; diff --git a/src/glass_time.rs b/src/glass_time.rs new file mode 100644 index 0000000..7814693 --- /dev/null +++ b/src/glass_time.rs @@ -0,0 +1,320 @@ +use std::net::{TcpListener, TcpStream}; +use std::sync::{Arc, Mutex}; +use std::thread; +use std::io::{Read, Write}; +use crossbeam_channel::{Sender, unbounded}; +use serde::Serialize; + +/// SHA-1 implementation to calculate the WebSocket Accept Key without external dependencies +fn sha1(input: &str) -> [u8; 20] { + let mut h0: u32 = 0x67452301; + let mut h1: u32 = 0xEFCDAB89; + let mut h2: u32 = 0x98BADCFE; + let mut h3: u32 = 0x10325476; + let mut h4: u32 = 0xC3D2E1F0; + + let bytes = input.as_bytes(); + let bit_len = (bytes.len() as u64) * 8; + + let mut padded = bytes.to_vec(); + padded.push(0x80); + while (padded.len() + 8) % 64 != 0 { + padded.push(0x00); + } + + for shift in (0..8).rev() { + padded.push(((bit_len >> (shift * 8)) & 0xFF) as u8); + } + + for chunk in padded.chunks_exact(64) { + let mut w = [0u32; 80]; + for i in 0..16 { + w[i] = ((chunk[i * 4] as u32) << 24) + | ((chunk[i * 4 + 1] as u32) << 16) + | ((chunk[i * 4 + 2] as u32) << 8) + | (chunk[i * 4 + 3] as u32); + } + for i in 16..80 { + w[i] = (w[i - 3] ^ w[i - 8] ^ w[i - 14] ^ w[i - 16]).rotate_left(1); + } + + let mut a = h0; + let mut b = h1; + let mut c = h2; + let mut d = h3; + let mut e = h4; + + for i in 0..80 { + let (f, k) = if i < 20 { + ((b & c) | (!b & d), 0x5A827999) + } else if i < 40 { + (b ^ c ^ d, 0x6ED9EBA1) + } else if i < 60 { + ((b & c) | (b & d) | (c & d), 0x8F1BBCDC) + } else { + (b ^ c ^ d, 0xCA62C1D6) + }; + + let temp = a.rotate_left(5) + .wrapping_add(f) + .wrapping_add(e) + .wrapping_add(k) + .wrapping_add(w[i]); + e = d; + d = c; + c = b.rotate_left(30); + b = a; + a = temp; + } + + h0 = h0.wrapping_add(a); + h1 = h1.wrapping_add(b); + h2 = h2.wrapping_add(c); + h3 = h3.wrapping_add(d); + h4 = h4.wrapping_add(e); + } + + let mut result = [0u8; 20]; + for (i, &val) in [h0, h1, h2, h3, h4].iter().enumerate() { + result[i * 4] = ((val >> 24) & 0xFF) as u8; + result[i * 4 + 1] = ((val >> 16) & 0xFF) as u8; + result[i * 4 + 2] = ((val >> 8) & 0xFF) as u8; + result[i * 4 + 3] = (val & 0xFF) as u8; + } + result +} + +/// Base64 encoder to format the WebSocket Accept Key +fn base64_encode(input: &[u8]) -> String { + const CHARSET: &[u8; 64] = b"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + let mut result = String::with_capacity((input.len() + 2) / 3 * 4); + for chunk in input.chunks(3) { + match chunk.len() { + 3 => { + let b0 = chunk[0] as usize; + let b1 = chunk[1] as usize; + let b2 = chunk[2] as usize; + result.push(CHARSET[b0 >> 2] as char); + result.push(CHARSET[((b0 & 0x03) << 4) | (b1 >> 4)] as char); + result.push(CHARSET[((b1 & 0x0F) << 2) | (b2 >> 6)] as char); + result.push(CHARSET[b2 & 0x3F] as char); + } + 2 => { + let b0 = chunk[0] as usize; + let b1 = chunk[1] as usize; + result.push(CHARSET[b0 >> 2] as char); + result.push(CHARSET[((b0 & 0x03) << 4) | (b1 >> 4)] as char); + result.push(CHARSET[(b1 & 0x0F) << 2] as char); + result.push('='); + } + 1 => { + let b0 = chunk[0] as usize; + result.push(CHARSET[b0 >> 2] as char); + result.push(CHARSET[(b0 & 0x03) << 4] as char); + result.push('='); + result.push('='); + } + _ => unreachable!(), + } + } + result +} + +/// Encodes a WebSocket text frame from a payload string +fn encode_ws_frame(payload: &str) -> Vec { + let bytes = payload.as_bytes(); + let len = bytes.len(); + let mut frame = Vec::new(); + + // Fin=1, Opcode=1 (Text) + frame.push(0x81); + + if len <= 125 { + frame.push(len as u8); + } else if len <= 65535 { + frame.push(126); + frame.push(((len >> 8) & 0xFF) as u8); + frame.push((len & 0xFF) as u8); + } else { + frame.push(127); + for shift in (0..8).rev() { + frame.push(((len >> (shift * 8)) & 0xFF) as u8); + } + } + + frame.extend_from_slice(bytes); + frame +} + +/// Handles the WebSocket handshake with a TCP client +fn handle_handshake(stream: &mut TcpStream) -> Result<(), std::io::Error> { + let mut buf = [0u8; 2048]; + let n = stream.read(&mut buf)?; + let request = String::from_utf8_lossy(&buf[..n]); + + let key_header = "Sec-WebSocket-Key: "; + if let Some(pos) = request.find(key_header) { + let start = pos + key_header.len(); + if let Some(end) = request[start..].find("\r\n") { + let key = request[start..start+end].trim(); + let accept_val = base64_encode(&sha1(&format!("{}{}", key, "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"))); + + let response = format!( + "HTTP/1.1 101 Switching Protocols\r\n\ + Upgrade: websocket\r\n\ + Connection: Upgrade\r\n\ + Sec-WebSocket-Accept: {}\r\n\r\n", + accept_val + ); + stream.write_all(response.as_bytes())?; + return Ok(()); + } + } + Err(std::io::Error::new(std::io::ErrorKind::InvalidData, "Invalid WebSocket Handshake")) +} + +#[derive(Serialize, Clone, Debug)] +pub struct TelemetryFrame { + pub timestamp: String, + pub rx_position: [f64; 3], + pub rx_velocity: [f64; 3], + pub clock_bias: f64, + pub clock_drift: f64, + pub active_channels: Vec, +} + +#[derive(Serialize, Clone, Debug)] +pub struct ChannelTelem { + pub id: usize, + pub sat_name: String, + pub status: String, + pub target_freq: f64, + pub freq_offset: f64, + pub snr: f64, + pub tec: f64, + pub s4: f64, + pub sigma_phi: f64, + pub is_dual: bool, + pub sat_position: [f64; 3], +} + +pub struct GlassTimeServer { + pub telemetry_tx: Sender, +} + +impl GlassTimeServer { + pub fn start(port: u16) -> Self { + let (telemetry_tx, telemetry_rx) = unbounded::(); + let clients = Arc::new(Mutex::new(Vec::new())); + let clients_clone = clients.clone(); + + // Spawn a thread to accept incoming TCP WebSocket connections + thread::spawn(move || { + let listener = match TcpListener::bind(format!("0.0.0.0:{}", port)) { + Ok(l) => l, + Err(e) => { + tracing::warn!("Failed to bind WebSocket server on port {}: {}", port, e); + return; + } + }; + listener.set_nonblocking(false).ok(); + + for stream in listener.incoming() { + if let Ok(mut stream) = stream { + let clients_list = clients_clone.clone(); + thread::spawn(move || { + if handle_handshake(&mut stream).is_ok() { + stream.set_nonblocking(true).ok(); + if let Ok(mut list) = clients_list.lock() { + list.push(stream); + } + } + }); + } + } + }); + + // Spawn a thread to broadcast serialized telemetry frames to all clients + thread::spawn(move || { + for frame in telemetry_rx { + if let Ok(payload) = serde_json::to_string(&frame) { + let frame_bytes = encode_ws_frame(&payload); + if let Ok(mut list) = clients.lock() { + let mut active_clients = Vec::new(); + for mut client in list.drain(..) { + if client.write_all(&frame_bytes).is_ok() { + active_clients.push(client); + } + } + *list = active_clients; + } + } + } + }); + + Self { telemetry_tx } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_sha1_and_base64() { + let hash = sha1("dGhlIHNhbXBsZSBub25jZQ==258EAFA5-E914-47DA-95CA-C5AB0DC85B11"); + let b64 = base64_encode(&hash); + assert_eq!(b64, "s3pPLMBiTxaQ9kYGzzhZRbK+xOo="); + } + + #[test] + fn test_websocket_server_e2e() { + let server = GlassTimeServer::start(19876); + std::thread::sleep(std::time::Duration::from_millis(100)); + + let mut client = TcpStream::connect("127.0.0.1:19876").expect("Failed to connect client"); + + let handshake = "GET / HTTP/1.1\r\n\ + Host: 127.0.0.1:19876\r\n\ + Upgrade: websocket\r\n\ + Connection: Upgrade\r\n\ + Sec-WebSocket-Key: dGhlIHNhbXBsZSBub25jZQ==\r\n\ + Sec-WebSocket-Version: 13\r\n\r\n"; + client.write_all(handshake.as_bytes()).expect("Failed to send handshake"); + + let mut buf = [0u8; 1024]; + let n = client.read(&mut buf).expect("Failed to read response"); + let resp = String::from_utf8_lossy(&buf[..n]); + assert!(resp.contains("HTTP/1.1 101 Switching Protocols")); + assert!(resp.contains("Sec-WebSocket-Accept: s3pPLMBiTxaQ9kYGzzhZRbK+xOo=")); + + let frame = TelemetryFrame { + timestamp: "2026-06-13T00:00:00Z".to_string(), + rx_position: [1.0, 2.0, 3.0], + rx_velocity: [4.0, 5.0, 6.0], + clock_bias: 1e-6, + clock_drift: 1.2, + active_channels: vec![], + }; + server.telemetry_tx.send(frame).expect("Failed to send telemetry frame"); + + std::thread::sleep(std::time::Duration::from_millis(100)); + let n2 = client.read(&mut buf).expect("Failed to read frame"); + + assert_eq!(buf[0], 0x81); + let len_byte = buf[1]; + assert!(len_byte > 0); + + let payload_start = if len_byte <= 125 { + 2 + } else if len_byte == 126 { + 4 + } else { + 10 + }; + + let payload = String::from_utf8_lossy(&buf[payload_start..n2]); + assert!(payload.contains("2026-06-13T00:00:00Z")); + assert!(payload.contains("clock_drift")); + } +} diff --git a/src/lib.rs b/src/lib.rs index 397e4ed..725eeb4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -4,6 +4,9 @@ pub mod ekf; pub mod orbit; pub mod orbit_solver; pub mod tui; +pub mod nav_ekf; +pub mod space_weather; +pub mod glass_time; use clap::Parser; use serde::{Deserialize, Serialize}; diff --git a/src/main.rs b/src/main.rs index 79d9306..73b6be6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -917,7 +917,7 @@ fn main() { std::process::exit(1); } - let pos_obs = wgs84_to_ecef(args.lat, args.lon, args.alt); + let mut pos_obs = wgs84_to_ecef(args.lat, args.lon, args.alt); // If simulation mode, generate raw IQ bytes and output to stdout if args.simulate { @@ -1525,7 +1525,11 @@ fn main() { .recv() .unwrap_or_else(|_| vec![Complex::new(0.0f32, 0.0f32); 32768]); if buf.len() < 32768 { - buf.resize(32768, Complex::new(0.0f32, 0.0f32)); + if buf.capacity() >= 32768 { + unsafe { buf.set_len(32768); } + } else { + buf.resize(32768, Complex::new(0.0f32, 0.0f32)); + } } let mut slice = &mut buf[..]; @@ -1646,6 +1650,7 @@ fn main() { let mut fft_scratch = vec![Complex::new(0.0f32, 0.0f32); fft.get_inplace_scratch_len()]; let mut step_count = 0; + let mut last_geo_solve_time: Option> = None; let mut start_system_time = if args.sim_start_time && !satellites.is_empty() { let (_sat_name, orbit) = &satellites[0]; if let Some(pca_time) = find_pca_time(orbit, pos_obs, orbit.epoch()) { @@ -1974,6 +1979,8 @@ fn main() { let mut mixed_scratch = vec![Complex::new(0.0f32, 0.0f32); 32768]; let mut main_eca_canceler = crate::dsp::EcaCanceler::new(); let mut eca_cleaned_samples = Vec::new(); + let mut master_nav_ekf = sattime::nav_ekf::MasterNavEkf::new(pos_obs); + let glass_time_server = sattime::glass_time::GlassTimeServer::start(9002); for samples in &rx { if !running.load(std::sync::atomic::Ordering::Relaxed) { @@ -2236,7 +2243,11 @@ fn main() { + chrono::Duration::microseconds((current_elapsed_seconds * 1e6) as i64); if mixed_scratch.len() != samples.len() { - mixed_scratch.resize(samples.len(), Complex::new(0.0f32, 0.0f32)); + if mixed_scratch.capacity() >= samples.len() { + unsafe { mixed_scratch.set_len(samples.len()); } + } else { + mixed_scratch.resize(samples.len(), Complex::new(0.0f32, 0.0f32)); + } } let mut primary_updated = false; @@ -2252,7 +2263,11 @@ fn main() { raw_snr_db = 0.0f32; if eca_cleaned_samples.len() != samples.len() { - eca_cleaned_samples.resize(samples.len(), Complex::new(0.0f32, 0.0f32)); + if eca_cleaned_samples.capacity() >= samples.len() { + unsafe { eca_cleaned_samples.set_len(samples.len()); } + } else { + eca_cleaned_samples.resize(samples.len(), Complex::new(0.0f32, 0.0f32)); + } } let samples_to_process = if !args.no_eca { @@ -2262,6 +2277,47 @@ fn main() { &samples }; + // Steer channel target frequencies using EKF predicted state + let dt = (samples.len() as f64) / args.sample_rate; + master_nav_ekf.predict(dt); + + for ch in &mut channels { + if ch.status != ChannelStatus::Idle { + if let Some(ref orbit) = ch.orbit { + if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(current_step_time) { + let rx_x = master_nav_ekf.x[0]; + let rx_y = master_nav_ekf.x[1]; + let rx_z = master_nav_ekf.x[2]; + let rx_vx = master_nav_ekf.x[3]; + let rx_vy = master_nav_ekf.x[4]; + let rx_vz = master_nav_ekf.x[5]; + let clk_drift = master_nav_ekf.x[7]; + + let dx = pos_sat[0] - rx_x; + let dy = pos_sat[1] - rx_y; + let dz = pos_sat[2] - rx_z; + let dist = (dx * dx + dy * dy + dz * dz).sqrt(); + if dist > 1.0 { + let ux = dx / dist; + let uy = dy / dist; + let uz = dz / dist; + + let range_rate = ux * (rx_vx - vel_sat[0]) + + uy * (rx_vy - vel_sat[1]) + + uz * (rx_vz - vel_sat[2]); + + let doppler_mult = 1.0 - range_rate / sattime::nav_ekf::C; + let f_c = if ch.initial_freq > 0.0 { ch.initial_freq } else { current_freq }; + ch.target_freq = f_c * (doppler_mult - clk_drift); + if ch.is_dual && ch.frequency2 > 0.0 { + ch.target_freq2 = ch.frequency2 * (doppler_mult - clk_drift); + } + } + } + } + } + } + process_pipeline_parallel(&mut channels, samples_to_process, current_freq, args.sample_rate); for ch in &mut channels { @@ -2346,13 +2402,180 @@ fn main() { tracking_bank = channels[0].tracking_bank.clone(); } + // Update Master EKF from active tracking channels + for ch in &mut channels { + if ch.status == ChannelStatus::Locked { + if let Some(ref orbit) = ch.orbit { + if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(current_step_time) { + // Check Čech Cohomology discrepancy as a topological firewall + let mut discrepancy = 0.0; + if let Some(ref bank) = ch.tracking_bank { + discrepancy = bank.compute_tracker_discrepancy() as f64; + } + if discrepancy > 150.0 { + // Spoofing or extreme multipath: skip updating the Master EKF to protect navigation state + continue; + } + + // Retrieve the active tracker (or primary PLL tracker) + let active_tracker = if let Some(ref bank) = ch.tracking_bank { + bank.active_idx + .map(|idx| &bank.trackers[idx]) + .unwrap_or(&ch.pll_tracker) + } else { + &ch.pll_tracker + }; + + let scale = if active_tracker.modulation == Modulation::Bpsk { 2.0 } else { 1.0 }; + let phase_residual = active_tracker.last_innovation / scale; + let ch_freq_offset = (active_tracker.x[1] / scale) / (2.0 * std::f64::consts::PI); + + let snr_linear = 10.0f64.powf((ch.snr as f64) / 10.0).max(0.1); + let lock_metric = active_tracker.lock_metric.clamp(1e-3, 1.0); + let noise_scale = 1.0 / (lock_metric * lock_metric * snr_linear); + + let f_c = if ch.initial_freq > 0.0 { ch.initial_freq } else { current_freq }; + master_nav_ekf.update_channel( + pos_sat, + vel_sat, + phase_residual, + ch_freq_offset, + f_c, + noise_scale, + ); + } + } + } + } + + // Update observer position dynamically from Master EKF + pos_obs[0] = master_nav_ekf.x[0]; + pos_obs[1] = master_nav_ekf.x[1]; + pos_obs[2] = master_nav_ekf.x[2]; + + // Serialize and broadcast real-time telemetry to WebSocket clients + let mut active_channels = Vec::new(); + for ch in &mut channels { + if ch.status != ChannelStatus::Idle { + let sat_pos = if let Some(ref orbit) = ch.orbit { + if let Some((pos_sat, _)) = orbit.propagate_ecef(current_step_time) { + pos_sat + } else { + [0.0, 0.0, 0.0] + } + } else { + [0.0, 0.0, 0.0] + }; + + ch.amp_history.make_contiguous(); + let active_tracker = if let Some(ref mut bank) = ch.tracking_bank { + bank.active_idx + .map(|idx| &mut bank.trackers[idx]) + .unwrap_or(&mut ch.pll_tracker) + } else { + &mut ch.pll_tracker + }; + active_tracker.innovation_history.make_contiguous(); + + let s4 = sattime::space_weather::compute_s4(ch.amp_history.as_slices().0); + let sigma_phi = sattime::space_weather::compute_sigma_phi(active_tracker.innovation_history.as_slices().0); + + let scale = if active_tracker.modulation == Modulation::Bpsk { 2.0 } else { 1.0 }; + let ch_freq_offset = (active_tracker.x[1] / scale) / (2.0 * std::f64::consts::PI); + + active_channels.push(sattime::glass_time::ChannelTelem { + id: ch.id, + sat_name: ch.sat_name.clone(), + status: format!("{:?}", ch.status), + target_freq: ch.target_freq, + freq_offset: ch_freq_offset, + snr: ch.snr, + tec: ch.current_tec, + s4, + sigma_phi, + is_dual: ch.is_dual, + sat_position: sat_pos, + }); + } + } + + let frame = sattime::glass_time::TelemetryFrame { + timestamp: current_step_time.to_rfc3339(), + rx_position: [master_nav_ekf.x[0], master_nav_ekf.x[1], master_nav_ekf.x[2]], + rx_velocity: [master_nav_ekf.x[3], master_nav_ekf.x[4], master_nav_ekf.x[5]], + clock_bias: master_nav_ekf.x[6], + clock_drift: master_nav_ekf.x[7], + active_channels, + }; + let _ = glass_time_server.telemetry_tx.send(frame); + + // Run RealTimeGeoSolver at 1 Hz if >= 4 channels are locked + let locked_count = channels.iter().filter(|ch| ch.status == ChannelStatus::Locked).count(); + if locked_count >= 4 { + let now_utc = chrono::Utc::now(); + let should_run = match last_geo_solve_time { + None => true, + Some(last) => (now_utc - last).num_seconds() >= 1, + }; + if should_run { + last_geo_solve_time = Some(now_utc); + let mut measurements = Vec::new(); + for ch in &channels { + if ch.status == ChannelStatus::Locked { + if let Some(ref orbit) = ch.orbit { + if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(current_step_time) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let dist = (dx * dx + dy * dy + dz * dz).sqrt(); + + // True range + let range = dist; + + measurements.push(( + ECEFCoordinates { x: pos_sat[0], y: pos_sat[1], z: pos_sat[2] }, + Velocity { vx: vel_sat[0], vy: vel_sat[1], vz: vel_sat[2] }, + range, + )); + } + } + } + } + if measurements.len() >= 4 { + let mut solver = RealTimeGeoSolver::new(); + solver.apply_troposphere = true; + if let Some(solved_coords) = solver.update_position(&measurements) { + // Print solved coordinates to logs + tracing::info!( + "[GEODESOLVER] Solved user position: Lat = {:.6}°, Lon = {:.6}°, Alt = {:.1}m", + solved_coords.latitude, + solved_coords.longitude, + solved_coords.altitude + ); + // Store the result in the global static GEOLOCATION_RESULT + let mut geo_res = get_geolocation_result().lock().unwrap(); + geo_res.lat = solved_coords.latitude; + geo_res.lon = solved_coords.longitude; + geo_res.alt = solved_coords.altitude; + geo_res.converged = true; + } else { + tracing::warn!("[GEODESOLVER] Solver failed to converge."); + } + } + } + } + // Now run the raw input decimation for spectrum visualization and main TUI draw decimated_samples.clear(); raw_decimator.process(&samples, &mut decimated_samples); let mut recycled_buf = samples; - recycled_buf.clear(); - recycled_buf.resize(32768, Complex::new(0.0f32, 0.0f32)); + if recycled_buf.capacity() >= 32768 { + unsafe { recycled_buf.set_len(32768); } + } else { + recycled_buf.clear(); + recycled_buf.resize(32768, Complex::new(0.0f32, 0.0f32)); + } let _ = pool_tx.send(recycled_buf); if decimated_samples.is_empty() { @@ -2389,6 +2612,8 @@ fn main() { } } + EnvelopeWaveletSpurCanceller::notch_spurs_wavelet(&mut fft_mag_ema, 0.0, 0.0); + if step_count > 0 && step_count % 1000 == 0 { let mut sorted = Vec::with_capacity(search_indices.len()); for &k in &search_indices { diff --git a/src/nav_ekf.rs b/src/nav_ekf.rs new file mode 100644 index 0000000..96a17c4 --- /dev/null +++ b/src/nav_ekf.rs @@ -0,0 +1,224 @@ +use nalgebra::{SMatrix, SVector}; + +/// Speed of light in m/s +pub const C: f64 = 299792458.0; + +#[derive(Clone, Debug)] +pub struct MasterNavEkf { + /// State vector: [X, Y, Z, Vx, Vy, Vz, dt_clk, df_clk]^T + /// Coordinates are in ECEF meters and m/s. + /// dt_clk is clock bias in seconds. + /// df_clk is clock drift in seconds/second. + pub x: SVector, + + /// Error covariance matrix + pub p: SMatrix, + + /// Process noise spectral densities + pub q_acc: f64, // acceleration random walk spectral density (m^2/s^3) + pub q_clk_bias: f64, // clock bias random walk spectral density (s^2/s) + pub q_clk_drift: f64, // clock drift random walk spectral density (s^2/s^3) + + /// Measurement noise floor values + pub r_phase_base: f64, // baseline carrier phase/range measurement variance (m^2) + pub r_freq_base: f64, // baseline Doppler/range rate measurement variance (m^2/s^2) +} + +impl MasterNavEkf { + pub fn new(init_pos: [f64; 3]) -> Self { + let mut x = SVector::::zeros(); + x[0] = init_pos[0]; + x[1] = init_pos[1]; + x[2] = init_pos[2]; + + let mut p = SMatrix::::zeros(); + // 100 meters uncertainty in initial position + p[(0, 0)] = 1e4; + p[(1, 1)] = 1e4; + p[(2, 2)] = 1e4; + // 10 m/s uncertainty in initial velocity + p[(3, 3)] = 1e2; + p[(4, 4)] = 1e2; + p[(5, 5)] = 1e2; + // 1 second uncertainty in initial clock bias + p[(6, 6)] = 1.0; + // 10 PPM (1e-5) uncertainty in initial clock drift + p[(7, 7)] = 1e-10; + + Self { + x, + p, + q_acc: 1.0, // 1 m^2/s^3 velocity walk + q_clk_bias: 1e-12, // TCXO phase walk + q_clk_drift: 1e-14, // TCXO drift walk + r_phase_base: 0.0025, // (0.05 m)^2 + r_freq_base: 0.04, // (0.2 m/s)^2 + } + } + + /// Propagate state covariance and state vector by time step dt (seconds) + pub fn predict(&mut self, dt: f64) { + if dt <= 0.0 { + return; + } + + // 1. Propagate state using Newtonian kinematics + let mut f = SMatrix::::identity(); + f[(0, 3)] = dt; + f[(1, 4)] = dt; + f[(2, 5)] = dt; + f[(6, 7)] = dt; + + self.x = f * self.x; + + // 2. Compute process noise matrix Q + let mut q = SMatrix::::zeros(); + let dt2 = dt * dt; + let dt3_3 = dt2 * dt / 3.0; + let dt2_2 = dt2 / 2.0; + + // Kinematic state process noise from velocity random walk + for i in 0..3 { + q[(i, i)] = self.q_acc * dt3_3; + q[(i, i + 3)] = self.q_acc * dt2_2; + q[(i + 3, i)] = self.q_acc * dt2_2; + q[(i + 3, i + 3)] = self.q_acc * dt; + } + + // Clock state process noise + q[(6, 6)] = self.q_clk_bias * dt + self.q_clk_drift * dt3_3; + q[(6, 7)] = self.q_clk_drift * dt2_2; + q[(7, 6)] = self.q_clk_drift * dt2_2; + q[(7, 7)] = self.q_clk_drift * dt; + + // Propagate covariance + self.p = f * self.p * f.transpose() + q; + + // Force symmetry to maintain numerical stability + self.p = (self.p + self.p.transpose()) * 0.5; + } + + /// Perform sequential 1D measurement update for a single channel's residuals + pub fn update_channel( + &mut self, + sat_pos: [f64; 3], + _sat_vel: [f64; 3], + phase_residual_rad: f64, + freq_residual_hz: f64, + carrier_freq_hz: f64, + noise_scale: f64, + ) { + if carrier_freq_hz <= 0.0 { + return; + } + + let lambda = C / carrier_freq_hz; + + // 1. Geometry calculations + let dx = sat_pos[0] - self.x[0]; + let dy = sat_pos[1] - self.x[1]; + let dz = sat_pos[2] - self.x[2]; + let rho = (dx * dx + dy * dy + dz * dz).sqrt(); + if rho < 1.0 { + return; + } + + // Unit line-of-sight vector from receiver to satellite + let ux = dx / rho; + let uy = dy / rho; + let uz = dz / rho; + + // 2. Carrier phase (range) measurement update + // Wavelength conversion: 1 rad of phase = lambda / (2*pi) meters of range + let y_range = - (lambda / (2.0 * std::f64::consts::PI)) * phase_residual_rad; + let mut h_range = SVector::::zeros(); + h_range[0] = ux; + h_range[1] = uy; + h_range[2] = uz; + h_range[6] = C; // clock bias sensitivity (scaled by speed of light) + + let r_range = self.r_phase_base * noise_scale; + self.update_1d(y_range, &h_range, r_range); + + // 3. Doppler frequency (range rate) measurement update + // Wavelength conversion: 1 Hz of frequency = lambda meters/sec of range rate + let y_rate = - lambda * freq_residual_hz; + let mut h_rate = SVector::::zeros(); + h_rate[3] = ux; + h_rate[4] = uy; + h_rate[5] = uz; + h_rate[7] = C; // clock drift sensitivity (scaled by speed of light) + + let r_rate = self.r_freq_base * noise_scale; + self.update_1d(y_rate, &h_rate, r_rate); + } + + /// Internal 1-dimensional EKF update using Joseph form covariance propagation + fn update_1d(&mut self, y: f64, h: &SVector, r: f64) { + let s = (h.transpose() * self.p * h)[0] + r; + if s.abs() >= 1e-12 { + let k = (self.p * h) / s; + self.x += k * y; + let i = SMatrix::::identity(); + let a = i - k * h.transpose(); + self.p = a * self.p * a.transpose() + k * r * k.transpose(); + + // Force symmetry and enforce covariance floor + self.p = (self.p + self.p.transpose()) * 0.5; + for idx in 0..8 { + self.p[(idx, idx)] = self.p[(idx, idx)].max(1e-15); + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_master_nav_ekf_predict_and_update() { + let init_pos = [0.0, 0.0, 6378137.0]; + let mut ekf = MasterNavEkf::new(init_pos); + + // Verify initial state + assert_eq!(ekf.x[0], 0.0); + assert_eq!(ekf.x[1], 0.0); + assert_eq!(ekf.x[2], 6378137.0); + assert_eq!(ekf.x[3], 0.0); + assert_eq!(ekf.x[7], 0.0); + + // Record initial diagonal values of P + let init_cov_pos = ekf.p[(2, 2)]; + + // Set a small clock bias uncertainty in test to isolate position updates + ekf.p[(6, 6)] = 1e-16; + + // Predict + ekf.predict(1.0); + + // P variance should increase due to process noise + assert!(ekf.p[(2, 2)] > init_cov_pos); + + // Simulated satellite overhead at [0.0, 0.0, 7000000.0] + let sat_pos = [0.0, 0.0, 7000000.0]; + let sat_vel = [0.0, 2000.0, 0.0]; // Moving in Y direction at 2 km/s + + // Perform an update step + // phase_residual_rad = 1.5 rad, freq_residual_hz = 10.0 Hz + ekf.update_channel( + sat_pos, + sat_vel, + 1.5, + 10.0, + 150800000.0, + 1.0, + ); + + // Verify that covariance has decreased (converged) after the measurement update + assert!(ekf.p[(2, 2)] < init_cov_pos); + + // Check that state vector is updated + assert!(ekf.x[2] != 6378137.0); + } +} diff --git a/src/orbit.rs b/src/orbit.rs index e9265d5..88fb627 100644 --- a/src/orbit.rs +++ b/src/orbit.rs @@ -104,6 +104,44 @@ pub fn enu_to_az_el(enu: [f64; 3]) -> (f64, f64) { (az, el) } +pub fn apply_sagnac_correction( + pos_sat: [f64; 3], + vel_sat: [f64; 3], + pos_obs: [f64; 3], +) -> ([f64; 3], [f64; 3]) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let range = (dx * dx + dy * dy + dz * dz).sqrt(); + if range > 0.0 { + let tau = range / 299792458.0; + let omega_e = 7.2921151467e-5; + let theta_sagnac = -omega_e * tau; + let cos_t = theta_sagnac.cos(); + let sin_t = theta_sagnac.sin(); + let p_corr = [ + pos_sat[0] * cos_t + pos_sat[1] * sin_t, + -pos_sat[0] * sin_t + pos_sat[1] * cos_t, + pos_sat[2], + ]; + let v_corr = [ + vel_sat[0] * cos_t + vel_sat[1] * sin_t, + -vel_sat[0] * sin_t + vel_sat[1] * cos_t, + vel_sat[2], + ]; + (p_corr, v_corr) + } else { + (pos_sat, vel_sat) + } +} + +pub fn saastamoinen_tropospheric_delay(sat_ecef: [f64; 3], obs_ecef: [f64; 3]) -> f64 { + let enu = ecef_to_enu(sat_ecef, obs_ecef); + let (_, el) = enu_to_az_el(enu); + let sin_el = el.sin().max(0.01); + 2.3 / (sin_el + 0.00143) +} + pub fn solve_linear_system(mut a: Vec>, mut b: Vec) -> Option> { let n = b.len(); for i in 0..n { @@ -152,12 +190,13 @@ pub fn predict_freq_sample( ) -> Option { let dt_true = dt - chrono::Duration::microseconds((delta_t * 1e6) as i64); if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(dt_true) { - let rx = pos_sat[0] - pos_obs[0]; - let ry = pos_sat[1] - pos_obs[1]; - let rz = pos_sat[2] - pos_obs[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, pos_obs); + let rx = pos_sat_corr[0] - pos_obs[0]; + let ry = pos_sat_corr[1] - pos_obs[1]; + let rz = pos_sat_corr[2] - pos_obs[2]; let range = (rx * rx + ry * ry + rz * rz).sqrt(); if range > 0.0 { - let range_rate = (rx * vel_sat[0] + ry * vel_sat[1] + rz * vel_sat[2]) / range; + let range_rate = (rx * vel_sat_corr[0] + ry * vel_sat_corr[1] + rz * vel_sat_corr[2]) / range; let doppler_term = 1.0 - range_rate / 299792458.0; return Some(df0 + center_freq * doppler_term); } @@ -1074,7 +1113,7 @@ pub fn run_blind_solver_check( } } -pub fn datetime_to_jd(dt: DateTime) -> f64 { +pub fn datetime_to_jd(dt: DateTime) -> (f64, f64) { let year = dt.year() as f64; let month = dt.month() as f64; let day = dt.day() as f64; @@ -1084,7 +1123,6 @@ pub fn datetime_to_jd(dt: DateTime) -> f64 { let nanosecond = dt.nanosecond() as f64; let day_fraction = (hour + (minute + (second + nanosecond / 1e9) / 60.0) / 60.0) / 24.0; - let jd_day = day + day_fraction; let (y, m) = if month <= 2.0 { (year - 1.0, month + 12.0) @@ -1095,11 +1133,12 @@ pub fn datetime_to_jd(dt: DateTime) -> f64 { let a = (y / 100.0).floor(); let b = 2.0 - a + (a / 4.0).floor(); - (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + jd_day + b - 1524.5 + let jd_base = (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + day + b - 1524.5; + (jd_base, day_fraction) } -pub fn teme_to_ecef(jd: f64, pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { - let d = jd - 2451545.0; +pub fn teme_to_ecef(jd: (f64, f64), pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { + let d = (jd.0 - 2451545.0) + jd.1; let t = d / 36525.0; let mut gmst = 280.46061837 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000.0; @@ -1447,11 +1486,12 @@ pub fn fit_satellite( for &(dt, freq_meas) in data { let dt_true = dt - chrono::Duration::microseconds((delta_t * 1e6) as i64); if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(dt_true) { - let rx = pos_sat[0] - pos_obs[0]; - let ry = pos_sat[1] - pos_obs[1]; - let rz = pos_sat[2] - pos_obs[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, pos_obs); + let rx = pos_sat_corr[0] - pos_obs[0]; + let ry = pos_sat_corr[1] - pos_obs[1]; + let rz = pos_sat_corr[2] - pos_obs[2]; let range = (rx * rx + ry * ry + rz * rz).sqrt(); - let range_rate = (rx * vel_sat[0] + ry * vel_sat[1] + rz * vel_sat[2]) / range; + let range_rate = (rx * vel_sat_corr[0] + ry * vel_sat_corr[1] + rz * vel_sat_corr[2]) / range; let doppler_term = 1.0 - range_rate / 299792458.0; y.push(freq_meas - center_freq * doppler_term); @@ -1496,11 +1536,12 @@ pub fn fit_satellite( for &(dt, freq_meas) in data { let dt_true = dt - chrono::Duration::microseconds((delta_t * 1e6) as i64); if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(dt_true) { - let rx = pos_sat[0] - pos_obs[0]; - let ry = pos_sat[1] - pos_obs[1]; - let rz = pos_sat[2] - pos_obs[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, pos_obs); + let rx = pos_sat_corr[0] - pos_obs[0]; + let ry = pos_sat_corr[1] - pos_obs[1]; + let rz = pos_sat_corr[2] - pos_obs[2]; let range = (rx * rx + ry * ry + rz * rz).sqrt(); - let range_rate = (rx * vel_sat[0] + ry * vel_sat[1] + rz * vel_sat[2]) / range; + let range_rate = (rx * vel_sat_corr[0] + ry * vel_sat_corr[1] + rz * vel_sat_corr[2]) / range; let doppler_term = 1.0 - range_rate / 299792458.0; y.push(freq_meas - center_freq * doppler_term); @@ -1544,11 +1585,12 @@ pub fn fit_satellite( for &(dt, freq_meas) in data { let dt_true = dt - chrono::Duration::microseconds((delta_t * 1e6) as i64); if let Some((pos_sat, vel_sat)) = orbit.propagate_ecef(dt_true) { - let rx = pos_sat[0] - pos_obs[0]; - let ry = pos_sat[1] - pos_obs[1]; - let rz = pos_sat[2] - pos_obs[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, pos_obs); + let rx = pos_sat_corr[0] - pos_obs[0]; + let ry = pos_sat_corr[1] - pos_obs[1]; + let rz = pos_sat_corr[2] - pos_obs[2]; let range = (rx * rx + ry * ry + rz * rz).sqrt(); - let range_rate = (rx * vel_sat[0] + ry * vel_sat[1] + rz * vel_sat[2]) / range; + let range_rate = (rx * vel_sat_corr[0] + ry * vel_sat_corr[1] + rz * vel_sat_corr[2]) / range; let doppler_term = 1.0 - range_rate / 299792458.0; y.push(freq_meas - center_freq * doppler_term); @@ -1650,6 +1692,7 @@ pub struct GeodeticCoordinates { /// Requires ≥4 simultaneous satellite range measurements to solve for 3D position. pub struct RealTimeGeoSolver { pub initial_guess: ECEFCoordinates, + pub apply_troposphere: bool, } impl RealTimeGeoSolver { @@ -1661,6 +1704,7 @@ impl RealTimeGeoSolver { y: 0.0, z: 6378137.0, // WGS84 semi-major axis }, + apply_troposphere: false, } } @@ -1739,7 +1783,12 @@ impl RealTimeGeoSolver { continue; } - let residual = dist - range; + let delta_trop = if self.apply_troposphere { + saastamoinen_tropospheric_delay([sat.x, sat.y, sat.z], [x, y, z]) + } else { + 0.0 + }; + let residual = dist - (range - delta_trop); let jx = dx / dist; let jy = dy / dist; let jz = dz / dist; @@ -2284,3 +2333,141 @@ pub fn load_orbits(path: &str) -> io::Result> { } Ok(satellites) } + +/// Computes central gravity acceleration (Earth central gravitational potential) +pub fn central_gravity_acceleration(pos: [f64; 3]) -> [f64; 3] { + const MU: f64 = 3.986004418e14; // m^3/s^2 + let r2 = pos[0]*pos[0] + pos[1]*pos[1] + pos[2]*pos[2]; + let r = r2.sqrt(); + if r < 1e-3 { + return [0.0, 0.0, 0.0]; + } + let factor = -MU / (r2 * r); + [pos[0] * factor, pos[1] * factor, pos[2] * factor] +} + +/// 2nd-Order Leapfrog (Verlet) Symplectic Integrator +pub struct LeapfrogIntegrator; + +impl LeapfrogIntegrator { + pub fn step(pos: [f64; 3], vel: [f64; 3], dt: f64) -> ([f64; 3], [f64; 3]) { + // Half-step position: r_half = r_n + 0.5 * dt * v_n + let r_half = [ + pos[0] + 0.5 * dt * vel[0], + pos[1] + 0.5 * dt * vel[1], + pos[2] + 0.5 * dt * vel[2], + ]; + + // Acceleration at half-step + let a_half = central_gravity_acceleration(r_half); + + // Full-step velocity: v_{n+1} = v_n + dt * a_half + let v_next = [ + vel[0] + dt * a_half[0], + vel[1] + dt * a_half[1], + vel[2] + dt * a_half[2], + ]; + + // Full-step position: r_{n+1} = r_half + 0.5 * dt * v_{n+1} + let r_next = [ + r_half[0] + 0.5 * dt * v_next[0], + r_half[1] + 0.5 * dt * v_next[1], + r_half[2] + 0.5 * dt * v_next[2], + ]; + + (r_next, v_next) + } + + pub fn propagate(mut pos: [f64; 3], mut vel: [f64; 3], dt: f64, steps: usize) -> ([f64; 3], [f64; 3]) { + for _ in 0..steps { + let (next_pos, next_vel) = Self::step(pos, vel, dt); + pos = next_pos; + vel = next_vel; + } + (pos, vel) + } +} + +/// 4th-Order Runge-Kutta-Nyström (RKN) / Forest-Ruth Symplectic Integrator +pub struct RknSymplecticIntegrator; + +impl RknSymplecticIntegrator { + pub fn step(mut pos: [f64; 3], mut vel: [f64; 3], dt: f64) -> ([f64; 3], [f64; 3]) { + // Forest-Ruth coefficients + let theta = 1.3512071917951; + let c1 = theta / 2.0; + let c2 = (1.0 - theta) / 2.0; + let c3 = c2; + let c4 = c1; + let d1 = theta; + let d2 = 1.0 - 2.0 * theta; + let d3 = d1; + let d4 = 0.0; + + let cs = [c1, c2, c3, c4]; + let ds = [d1, d2, d3, d4]; + + for i in 0..4 { + // Stage update + pos[0] += cs[i] * dt * vel[0]; + pos[1] += cs[i] * dt * vel[1]; + pos[2] += cs[i] * dt * vel[2]; + + let a = central_gravity_acceleration(pos); + + vel[0] += ds[i] * dt * a[0]; + vel[1] += ds[i] * dt * a[1]; + vel[2] += ds[i] * dt * a[2]; + } + + (pos, vel) + } + + pub fn propagate(mut pos: [f64; 3], mut vel: [f64; 3], dt: f64, steps: usize) -> ([f64; 3], [f64; 3]) { + for _ in 0..steps { + let (next_pos, next_vel) = Self::step(pos, vel, dt); + pos = next_pos; + vel = next_vel; + } + (pos, vel) + } +} + +#[cfg(test)] +mod orbit_tests { + use super::*; + + #[test] + fn test_symplectic_integrators_energy_conservation() { + let mu: f64 = 3.986004418e14; + let r = 7178137.0; // Circular orbit at 800 km altitude + let v_circ = (mu / r).sqrt(); + + let pos_init = [r, 0.0, 0.0]; + let vel_init = [0.0, v_circ, 0.0]; + + let initial_energy = 0.5 * v_circ * v_circ - mu / r; + + let dt = 1.0; + let steps = 100_000; + + // 1. Leapfrog (2nd-order symplectic) + let (pos_lf, vel_lf) = LeapfrogIntegrator::propagate(pos_init, vel_init, dt, steps); + let lf_v2 = vel_lf[0]*vel_lf[0] + vel_lf[1]*vel_lf[1] + vel_lf[2]*vel_lf[2]; + let lf_r = (pos_lf[0]*pos_lf[0] + pos_lf[1]*pos_lf[1] + pos_lf[2]*pos_lf[2]).sqrt(); + let lf_energy = 0.5 * lf_v2 - mu / lf_r; + let lf_energy_err = (lf_energy - initial_energy).abs() / initial_energy.abs(); + + assert!(lf_energy_err < 1e-4, "Leapfrog energy relative error too high: {}", lf_energy_err); + + // 2. RKN (4th-order symplectic) + let (pos_rkn, vel_rkn) = RknSymplecticIntegrator::propagate(pos_init, vel_init, dt, steps); + let rkn_v2 = vel_rkn[0]*vel_rkn[0] + vel_rkn[1]*vel_rkn[1] + vel_rkn[2]*vel_rkn[2]; + let rkn_r = (pos_rkn[0]*pos_rkn[0] + pos_rkn[1]*pos_rkn[1] + pos_rkn[2]*pos_rkn[2]).sqrt(); + let rkn_energy = 0.5 * rkn_v2 - mu / rkn_r; + let rkn_energy_err = (rkn_energy - initial_energy).abs() / initial_energy.abs(); + + assert!(rkn_energy_err < 1e-6, "RKN energy relative error too high: {}", rkn_energy_err); + } +} + diff --git a/src/orbit_solver.rs b/src/orbit_solver.rs index 1ab9d5b..bb4989c 100644 --- a/src/orbit_solver.rs +++ b/src/orbit_solver.rs @@ -1,4 +1,4 @@ -use crate::orbit::{datetime_to_jd, teme_to_ecef}; +use crate::orbit::{datetime_to_jd, teme_to_ecef, apply_sagnac_correction}; use chrono::{DateTime, Datelike, Timelike, Utc}; use rayon::prelude::*; use std::fs::File; @@ -280,23 +280,25 @@ pub fn predict_frequency( let t_corr = t_adj + chrono::Duration::nanoseconds((dt_rel * 1e9) as i64); let (pos_sat, vel_sat) = propagate_ecef_at_time(a, i, raan0, u0, epoch, t_corr, t_obs); - let dx = pos_sat[0] - rec_ecef[0]; - let dy = pos_sat[1] - rec_ecef[1]; - let dz = pos_sat[2] - rec_ecef[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, rec_ecef); + + let dx = pos_sat_corr[0] - rec_ecef[0]; + let dy = pos_sat_corr[1] - rec_ecef[1]; + let dz = pos_sat_corr[2] - rec_ecef[2]; let dist = (dx * dx + dy * dy + dz * dz).sqrt(); if dist < 1.0 { return center_freq + df; } - let v_sat_sq = vel_sat[0] * vel_sat[0] + vel_sat[1] * vel_sat[1] + vel_sat[2] * vel_sat[2]; - let r_sat = (pos_sat[0] * pos_sat[0] + pos_sat[1] * pos_sat[1] + pos_sat[2] * pos_sat[2]).sqrt(); + let v_sat_sq = vel_sat_corr[0] * vel_sat_corr[0] + vel_sat_corr[1] * vel_sat_corr[1] + vel_sat_corr[2] * vel_sat_corr[2]; + let r_sat = (pos_sat_corr[0] * pos_sat_corr[0] + pos_sat_corr[1] * pos_sat_corr[1] + pos_sat_corr[2] * pos_sat_corr[2]).sqrt(); let r_rec = (rec_ecef[0] * rec_ecef[0] + rec_ecef[1] * rec_ecef[1] + rec_ecef[2] * rec_ecef[2]).sqrt(); let u_sat = -MU / r_sat; let u_rec = -MU / r_rec; - let v_dot_n = (vel_sat[0] * dx + vel_sat[1] * dy + vel_sat[2] * dz) / dist; + let v_dot_n = (vel_sat_corr[0] * dx + vel_sat_corr[1] * dy + vel_sat_corr[2] * dz) / dist; let gamma_inv = (1.0 - v_sat_sq / (C * C)).sqrt(); let denominator = 1.0 - v_dot_n / C; @@ -329,23 +331,25 @@ pub fn predict_frequency_poly( let t_corr = t_adj + chrono::Duration::nanoseconds((dt_rel * 1e9) as i64); let (pos_sat, vel_sat) = propagate_ecef_at_time(a, i, raan0, u0, epoch, t_corr, t_obs); - let dx = pos_sat[0] - rec_ecef[0]; - let dy = pos_sat[1] - rec_ecef[1]; - let dz = pos_sat[2] - rec_ecef[2]; + let (pos_sat_corr, vel_sat_corr) = apply_sagnac_correction(pos_sat, vel_sat, rec_ecef); + + let dx = pos_sat_corr[0] - rec_ecef[0]; + let dy = pos_sat_corr[1] - rec_ecef[1]; + let dz = pos_sat_corr[2] - rec_ecef[2]; let dist = (dx * dx + dy * dy + dz * dz).sqrt(); if dist < 1.0 { return center_freq + df_poly.0; } - let v_sat_sq = vel_sat[0] * vel_sat[0] + vel_sat[1] * vel_sat[1] + vel_sat[2] * vel_sat[2]; - let r_sat = (pos_sat[0] * pos_sat[0] + pos_sat[1] * pos_sat[1] + pos_sat[2] * pos_sat[2]).sqrt(); + let v_sat_sq = vel_sat_corr[0] * vel_sat_corr[0] + vel_sat_corr[1] * vel_sat_corr[1] + vel_sat_corr[2] * vel_sat_corr[2]; + let r_sat = (pos_sat_corr[0] * pos_sat_corr[0] + pos_sat_corr[1] * pos_sat_corr[1] + pos_sat_corr[2] * pos_sat_corr[2]).sqrt(); let r_rec = (rec_ecef[0] * rec_ecef[0] + rec_ecef[1] * rec_ecef[1] + rec_ecef[2] * rec_ecef[2]).sqrt(); let u_sat = -MU / r_sat; let u_rec = -MU / r_rec; - let v_dot_n = (vel_sat[0] * dx + vel_sat[1] * dy + vel_sat[2] * dz) / dist; + let v_dot_n = (vel_sat_corr[0] * dx + vel_sat_corr[1] * dy + vel_sat_corr[2] * dz) / dist; let gamma_inv = (1.0 - v_sat_sq / (C * C)).sqrt(); let denominator = 1.0 - v_dot_n / C; @@ -358,6 +362,154 @@ pub fn predict_frequency_poly( } +pub struct AdelicLangevinOptimizer { + primes: Vec, +} + +impl AdelicLangevinOptimizer { + pub fn new() -> Self { + Self { + primes: vec![2, 3, 5, 7], + } + } + + pub fn optimize( + &mut self, + initial_state: [f64; 4], + bounds: &[(f64, f64); 4], + mut cost_fn: F, + steps: usize, + rng: &mut SimpleRng, + ) -> ([f64; 4], f64) + where + F: FnMut(&[f64; 4]) -> f64, + { + let mut current_state = initial_state; + let mut current_rss = cost_fn(¤t_state); + + let mut best_state = current_state; + let mut best_rss = current_rss; + + let mut lr = 0.1; + let mut noise_std = 0.05; + + let mut rss_history = Vec::with_capacity(steps); + + for step in 0..steps { + rss_history.push(current_rss); + + // 1. Compute numerical gradient + let mut grad = [0.0; 4]; + for i in 0..4 { + let eps = if i == 0 { 1000.0 } else { 1e-4 }; + let mut state_plus = current_state; + state_plus[i] += eps; + let rss_plus = cost_fn(&state_plus); + + let mut state_minus = current_state; + state_minus[i] -= eps; + let rss_minus = cost_fn(&state_minus); + + grad[i] = (rss_plus - rss_minus) / (2.0 * eps); + } + + // 2. Perform Langevin step + let mut next_state = current_state; + for i in 0..4 { + let grad_sign = if grad[i].is_nan() { 0.0 } else { grad[i].signum() }; + + let (grad_scale, noise_scale) = if i == 0 { + (1000.0, 10.0) + } else if i == 1 { + (0.01, 0.001) + } else { + (0.2, 0.05) + }; + + let noise = rng.next_gaussian() * noise_std * noise_scale; + next_state[i] = current_state[i] - lr * grad_sign * grad_scale + noise; + next_state[i] = next_state[i].clamp(bounds[i].0, bounds[i].1); + } + + let next_rss = cost_fn(&next_state); + if next_rss < current_rss { + current_state = next_state; + current_rss = next_rss; + } + + // 3. Adelic Jump (only perturb periodic parameters raan0 and u0) + let p = self.primes[step % self.primes.len()]; + let mut jump_state = current_state; + + for i in 2..4 { + let range = bounds[i].1 - bounds[i].0; + if range > 0.0 { + let val_normalized = ((current_state[i] - bounds[i].0) / range).clamp(0.0, 0.99999); + let p_adic_val = inverse_monna_map(val_normalized, p, 16); + + let perturb_scale = 4; + let perturbation = (rng.next_f64() * (p as f64).powi(perturb_scale)) as u64; + let perturbed_p_adic = p_adic_val.wrapping_add(perturbation); + + let val_jump_normalized = monna_map(perturbed_p_adic, p); + let val_jump = bounds[i].0 + val_jump_normalized * range; + jump_state[i] = val_jump.clamp(bounds[i].0, bounds[i].1); + } + } + + let jump_rss = cost_fn(&jump_state); + + // Regularization check based on historical variation + let mut current_diffs = Vec::new(); + if rss_history.len() >= 2 { + for j in 1..rss_history.len() { + let prev = rss_history[j - 1]; + let curr = rss_history[j]; + if prev > 0.0 { + current_diffs.push((curr - prev) / prev); + } else { + current_diffs.push(0.0); + } + } + } + let current_var_sum: f64 = current_diffs.iter().map(|d| d.abs()).sum(); + let reg_rss_current = current_rss + 0.01 * current_var_sum; + + let mut temp_history = rss_history.clone(); + temp_history.push(jump_rss); + let mut jump_diffs = Vec::new(); + if temp_history.len() >= 2 { + for j in 1..temp_history.len() { + let prev = temp_history[j - 1]; + let curr = temp_history[j]; + if prev > 0.0 { + jump_diffs.push((curr - prev) / prev); + } else { + jump_diffs.push(0.0); + } + } + } + let jump_var_sum: f64 = jump_diffs.iter().map(|d| d.abs()).sum(); + let reg_rss_jump = jump_rss + 0.01 * jump_var_sum; + + if reg_rss_jump < reg_rss_current { + current_state = jump_state; + current_rss = jump_rss; + } + + if current_rss < best_rss { + best_rss = current_rss; + best_state = current_state; + } + + lr *= 0.95; + noise_std *= 0.9; + } + + (best_state, best_rss) + } +} + // Formulate the Keplerian parameters Levenberg-Marquardt solver pub fn fit_orbit_doppler( raw_passes: &[RawPass], @@ -424,27 +576,44 @@ pub fn fit_orbit_doppler( } let a_est = if obs_pcas.len() >= 2 { - let mut t_obs_sum = 0.0; - let mut t_obs_count = 0.0; - let t0 = obs_pcas[0]; - for &tp in &obs_pcas[1..] { - let dt = (tp - t0).num_milliseconds() as f64 / 1000.0; - let k = (dt / 5700.0).round(); - println!("DEBUG: dt={}, k={}, dt/k={}", dt, k, dt / k); - if k > 0.0 { - t_obs_sum += dt / k; - t_obs_count += 1.0; + let t_expected_sidereal = 2.0 * std::f64::consts::PI * (initial_a.powi(3) / MU).sqrt(); + let cos_i = initial_i.cos(); + let omega_e = 7.2921151467e-5; + let t_expected_synodic = t_expected_sidereal / (1.0 - (omega_e * t_expected_sidereal / (2.0 * std::f64::consts::PI)) * cos_i); + + let mut best_dt_k = 0.0; + let mut min_score = f64::MAX; + + for i in 0..obs_pcas.len() { + for j in i + 1..obs_pcas.len() { + let dt = (obs_pcas[j] - obs_pcas[i]).num_milliseconds() as f64 / 1000.0; + let k = (dt / t_expected_synodic).round(); + if k > 0.0 { + let dt_k = dt / k; + let diff = (dt_k - t_expected_synodic).abs(); + // Prioritize smaller k, and then smaller difference from expected synodic period + let score = k * 1000.0 + diff; + if score < min_score { + min_score = score; + best_dt_k = dt_k; + } + } } } - let t_observed = if t_obs_count > 0.0 { - t_obs_sum / t_obs_count + + let t_observed = if best_dt_k > 0.0 { + best_dt_k } else { - 5700.0 + t_expected_synodic }; - let a_val = (MU * t_observed * t_observed + + // Convert synodic t_observed to sidereal t_corrected + let t_corrected = t_observed / (1.0 + (omega_e * t_observed / (2.0 * std::f64::consts::PI)) * cos_i); + + let a_val = (MU * t_corrected * t_corrected / (4.0 * std::f64::consts::PI * std::f64::consts::PI)) .powf(1.0 / 3.0); - println!("DEBUG: t_observed={}, a_est={}", t_observed, a_val); + println!("DEBUG: t_observed_synodic={}, t_corrected_sidereal={}, a_est={}", t_observed, t_corrected, a_val); a_val.max(6500e3).min(20000e3) } else { initial_a @@ -453,13 +622,12 @@ pub fn fit_orbit_doppler( // --- STAGE 1: Fit using only the first 2 passes to get close to true a and i --- let stage1_passes = &raw_passes[0..2]; let mut stage1_params = vec![0.0; 4 + 2 * 2]; // 4 global + 2 * 2 pass-specific = 8 params - stage1_params[0] = a_est; + stage1_params[0] = initial_a; stage1_params[1] = initial_i; // Run Langevin Global Optimizer on the first 2 passes to get raan0 and u0 - let mut best_raan0 = 0.0_f64; - let mut best_u0 = 0.0_f64; - let mut best_rss = f64::MAX; + // Run Adelic Langevin Global Optimizer on the first 2 passes to get global Keplerian elements + // 12x12 grid of starting points for Langevin trajectories (30 degree spacing) @@ -472,7 +640,8 @@ pub fn fit_orbit_doppler( let mut rng = SimpleRng::new(1337); for &init_raan in &grid_points { for &init_u0 in &grid_points { - starts.push((init_raan, init_u0, rng.state)); + starts.push((initial_a, init_raan, init_u0, rng.state)); + starts.push((a_est, init_raan, init_u0, rng.state)); for _ in 0..75 { rng.next_f64(); } @@ -485,248 +654,113 @@ pub fn fit_orbit_doppler( .build() .unwrap(); - let results: Vec<((f64, f64), f64, Vec)> = pool.install(|| { + let results: Vec<([f64; 4], f64)> = pool.install(|| { starts .into_par_iter() - .map(|(init_raan, init_u0, seed_state)| { - let mut raan0 = init_raan; - let mut u0 = init_u0; - let mut traj_best_raan = raan0; - let mut traj_best_u = u0; - let mut traj_best_rss = f64::MAX; - + .map(|(start_a, init_raan, init_u0, seed_state)| { let mut rng = SimpleRng { state: seed_state }; - - let mut lr = 0.1; - let mut noise_std = 0.05; - - let mut rss_history = Vec::new(); - - for step in 0..15 { - let current_rss = compute_rss( - stage1_passes, - rec_ecef, - stage1_params[0], - stage1_params[1], - epoch, - center_freq, - raan0, - u0, - ); - rss_history.push(current_rss); - if current_rss < traj_best_rss { - traj_best_rss = current_rss; - traj_best_raan = raan0; - traj_best_u = u0; - } - - // Compute gradient - let (_, _, grad_raan, grad_u0) = compute_gradient( - stage1_passes, - rec_ecef, - stage1_params[0], - stage1_params[1], - epoch, - center_freq, - raan0, - u0, - ); - - let sign_raan = if grad_raan.is_nan() { - 0.0 - } else { - grad_raan.signum() - }; - let sign_u0 = if grad_u0.is_nan() { - 0.0 - } else { - grad_u0.signum() - }; - - // Update directions - let step_raan = -lr * sign_raan * 0.2 + noise_std * rng.next_gaussian() * 0.05; - let step_u0 = -lr * sign_u0 * 0.2 + noise_std * rng.next_gaussian() * 0.05; - - let next_raan0 = (raan0 + step_raan).rem_euclid(2.0 * std::f64::consts::PI); - let next_u0 = (u0 + step_u0).rem_euclid(2.0 * std::f64::consts::PI); - - raan0 = next_raan0; - u0 = next_u0; - - // Digit-scrambling restart using base-p digit reversal mapping (Monna map) - let primes = [2, 3, 5, 7]; - let p = primes[step % primes.len()]; - - let x_raan = raan0 / (2.0 * std::f64::consts::PI); - let x_u = u0 / (2.0 * std::f64::consts::PI); - - let val_raan = inverse_monna_map(x_raan, p, 16); - let val_u = inverse_monna_map(x_u, p, 16); - - let perturb_scale = 4; - let perturbation = (rng.next_f64() * (p as f64).powi(perturb_scale)) as u64; - let val_raan_perturbed = val_raan.wrapping_add(perturbation); - let val_u_perturbed = val_u.wrapping_add(perturbation); - - let x_raan_scrambled = monna_map(val_raan_perturbed, p); - let x_u_scrambled = monna_map(val_u_perturbed, p); - - let raan0_scrambled = (x_raan_scrambled * 2.0 * std::f64::consts::PI) - .rem_euclid(2.0 * std::f64::consts::PI); - let u0_scrambled = (x_u_scrambled * 2.0 * std::f64::consts::PI) - .rem_euclid(2.0 * std::f64::consts::PI); - - let scrambled_rss = compute_rss( + let mut opt = AdelicLangevinOptimizer::new(); + let bounds = [ + (start_a, start_a), // a + (initial_i, initial_i), // i + (0.0, 2.0 * std::f64::consts::PI), // raan0 + (0.0, 2.0 * std::f64::consts::PI), // u0 + ]; + let cost_fn = |state: &[f64; 4]| { + compute_rss( stage1_passes, rec_ecef, - stage1_params[0], - stage1_params[1], + state[0], + state[1], epoch, center_freq, - raan0_scrambled, - u0_scrambled, - ); - let current_diffs = compute_fractional_difference_history(&rss_history); - let current_var_sum: f64 = current_diffs.iter().map(|d| d.abs()).sum(); - let reg_rss_current = current_rss + 0.01 * current_var_sum; - - let mut temp_history = rss_history.clone(); - if let Some(last_elem) = temp_history.last_mut() { - *last_elem = scrambled_rss; - } - let scrambled_diffs = compute_fractional_difference_history(&temp_history); - let scrambled_var_sum: f64 = scrambled_diffs.iter().map(|d| d.abs()).sum(); - let reg_rss_scrambled = scrambled_rss + 0.01 * scrambled_var_sum; - - if reg_rss_scrambled < reg_rss_current { - raan0 = raan0_scrambled; - u0 = u0_scrambled; - if let Some(last_elem) = rss_history.last_mut() { - *last_elem = scrambled_rss; - } - } - - lr *= 0.95; - noise_std *= 0.9; - } - - let final_rss = compute_rss( - stage1_passes, - rec_ecef, - stage1_params[0], - stage1_params[1], - epoch, - center_freq, - raan0, - u0, - ); - if final_rss < traj_best_rss { - traj_best_rss = final_rss; - traj_best_raan = raan0; - traj_best_u = u0; - } - - ((traj_best_raan, traj_best_u), traj_best_rss, rss_history) + state[2], + state[3], + ) + }; + let initial_state = [start_a, initial_i, init_raan, init_u0]; + let (best_state, best_rss) = opt.optimize(initial_state, &bounds, cost_fn, 15, &mut rng); + (best_state, best_rss) }) .collect() }); - let mut best_rss_history = Vec::new(); - for ((traj_best_raan, traj_best_u), traj_best_rss, traj_history) in results { - if traj_best_rss < best_rss { - best_rss = traj_best_rss; - best_raan0 = traj_best_raan; - best_u0 = traj_best_u; - best_rss_history = traj_history; - } - } + let mut best_init_a_state = [initial_a, initial_i, 0.0, 0.0]; + let mut best_init_a_rss = f64::MAX; + let mut best_a_est_state = [a_est, initial_i, 0.0, 0.0]; + let mut best_a_est_rss = f64::MAX; - let rss_diffs = compute_fractional_difference_history(&best_rss_history); - if !rss_diffs.is_empty() { - println!( - "Langevin trajectory fractional variation sum: {:?}", - rss_diffs.iter().sum::() - ); + for (state, rss) in results { + if (state[0] - initial_a).abs() < 1.0 { + if rss < best_init_a_rss { + best_init_a_rss = rss; + best_init_a_state = state; + } + } else { + if rss < best_a_est_rss { + best_a_est_rss = rss; + best_a_est_state = state; + } + } } println!( - "Langevin best: raan0={:.4} deg, u0={:.4} deg, rss={:.2e}", - best_raan0.to_degrees(), - best_u0.to_degrees(), - best_rss + "Adelic Langevin best init_a: a={:.1}m, i={:.4} deg, raan0={:.4} deg, u0={:.4} deg, rss={:.2e}", + best_init_a_state[0], + best_init_a_state[1].to_degrees(), + best_init_a_state[2].to_degrees(), + best_init_a_state[3].to_degrees(), + best_init_a_rss ); - stage1_params[2] = best_raan0; - stage1_params[3] = best_u0; - - // Initialize stage 1 pass-specific parameters - let stage1_pred_pcas = get_pred_pca_times( - stage1_params[0], - stage1_params[1], - stage1_params[2], - stage1_params[3], - epoch, - rec_ecef, - stage1_passes, - ); - for (p_idx, pass) in stage1_passes.iter().enumerate() { - let mut obs_pca_time = pass.points[0].time; - let mut min_offset = f64::MAX; - for pt in &pass.points { - let off = (pt.freq - center_freq).abs(); - if off < min_offset { - min_offset = off; - obs_pca_time = pt.time; - } - } - let pred_pca_time = stage1_pred_pcas[p_idx]; - let dt = (pred_pca_time - obs_pca_time).num_milliseconds() as f64 / 1000.0; - stage1_params[4 + 2 * p_idx] = dt; - stage1_params[4 + 2 * p_idx + 1] = 0.0; + if best_a_est_rss < f64::MAX { + println!( + "Adelic Langevin best a_est: a={:.1}m, i={:.4} deg, raan0={:.4} deg, u0={:.4} deg, rss={:.2e}", + best_a_est_state[0], + best_a_est_state[1].to_degrees(), + best_a_est_state[2].to_degrees(), + best_a_est_state[3].to_degrees(), + best_a_est_rss + ); } - // Run LM on Stage 1 (optimize a, i, raan0, u0 using only the first 2 passes) - let mut stage1_lambda = 1.0; - let mut best_stage1_rss = f64::MAX; - let mut best_stage1_params = stage1_params.clone(); - - for _ in 0..100 { - let mut residuals = Vec::new(); + let run_stage1_lm = |best_state: [f64; 4]| -> (Vec, f64) { + let mut stage1_params = vec![0.0; 4 + 2 * 2]; + stage1_params[0] = best_state[0]; + stage1_params[1] = best_state[1]; + stage1_params[2] = best_state[2]; + stage1_params[3] = best_state[3]; + + let stage1_pred_pcas = get_pred_pca_times( + stage1_params[0], + stage1_params[1], + stage1_params[2], + stage1_params[3], + epoch, + rec_ecef, + stage1_passes, + ); for (p_idx, pass) in stage1_passes.iter().enumerate() { - let dt = stage1_params[4 + 2 * p_idx]; - let df = stage1_params[4 + 2 * p_idx + 1]; + let mut obs_pca_time = pass.points[0].time; + let mut min_offset = f64::MAX; for pt in &pass.points { - let pred = predict_frequency( - stage1_params[0], - stage1_params[1], - stage1_params[2], - stage1_params[3], - epoch, - pt.time, - dt, - df, - center_freq, - rec_ecef, - ); - residuals.push(pt.freq - (center_freq + pred)); + let off = (pt.freq - center_freq).abs(); + if off < min_offset { + min_offset = off; + obs_pca_time = pt.time; + } } - } - for p_idx in 0..stage1_passes.len() { - let dt = stage1_params[4 + 2 * p_idx]; - residuals.push(dt * 10.0); + let pred_pca_time = stage1_pred_pcas[p_idx]; + let dt = (pred_pca_time - obs_pca_time).num_milliseconds() as f64 / 1000.0; + stage1_params[4 + 2 * p_idx] = dt; + stage1_params[4 + 2 * p_idx + 1] = 0.0; } - let rss: f64 = residuals.iter().map(|r| r * r).sum(); - if rss < best_stage1_rss { - best_stage1_rss = rss; - best_stage1_params = stage1_params.clone(); - stage1_lambda /= 10.0; - } else { - stage1_params = best_stage1_params.clone(); - stage1_lambda *= 10.0; - if stage1_lambda > 1e12 { - break; - } - residuals.clear(); + let mut stage1_lambda = 1.0; + let mut best_stage1_rss = f64::MAX; + let mut best_stage1_params = stage1_params.clone(); + + for _ in 0..100 { + let mut residuals = Vec::new(); for (p_idx, pass) in stage1_passes.iter().enumerate() { let dt = stage1_params[4 + 2 * p_idx]; let df = stage1_params[4 + 2 * p_idx + 1]; @@ -750,91 +784,169 @@ pub fn fit_orbit_doppler( let dt = stage1_params[4 + 2 * p_idx]; residuals.push(dt * 10.0); } - } - - let n_obs = residuals.len(); - if n_obs < 8 { - break; - } - let mut jacobian = vec![vec![0.0; 8]; n_obs]; - for k in 0..8 { - let mut perturbed = stage1_params.clone(); - let param_eps = if k == 0 { - 10.0 - } else if k == 1 || k == 2 || k == 3 { - 1e-6 - } else if (k - 4) % 2 == 0 { - 1e-3 + let rss: f64 = residuals.iter().map(|r| r * r).sum(); + if rss < best_stage1_rss { + best_stage1_rss = rss; + best_stage1_params = stage1_params.clone(); + stage1_lambda /= 10.0; } else { - 1e-2 - }; - perturbed[k] += param_eps; + stage1_params = best_stage1_params.clone(); + stage1_lambda *= 10.0; + if stage1_lambda > 1e12 { + break; + } + residuals.clear(); + for (p_idx, pass) in stage1_passes.iter().enumerate() { + let dt = stage1_params[4 + 2 * p_idx]; + let df = stage1_params[4 + 2 * p_idx + 1]; + for pt in &pass.points { + let pred = predict_frequency( + stage1_params[0], + stage1_params[1], + stage1_params[2], + stage1_params[3], + epoch, + pt.time, + dt, + df, + center_freq, + rec_ecef, + ); + residuals.push(pt.freq - (center_freq + pred)); + } + } + for p_idx in 0..stage1_passes.len() { + let dt = stage1_params[4 + 2 * p_idx]; + residuals.push(dt * 10.0); + } + } - let mut row_idx = 0; - for (p_idx, pass) in stage1_passes.iter().enumerate() { - let dt = perturbed[4 + 2 * p_idx]; - let df = perturbed[4 + 2 * p_idx + 1]; - for pt in &pass.points { - let pred = predict_frequency( - perturbed[0], - perturbed[1], - perturbed[2], - perturbed[3], - epoch, - pt.time, - dt, - df, - center_freq, - rec_ecef, - ); - let diff = pt.freq - (center_freq + pred); + let n_obs = residuals.len(); + if n_obs < 8 { + break; + } + + let mut jacobian = vec![vec![0.0; 8]; n_obs]; + for k in 0..8 { + let mut perturbed = stage1_params.clone(); + let param_eps = if k == 0 { + 10.0 + } else if k == 1 || k == 2 || k == 3 { + 1e-6 + } else if (k - 4) % 2 == 0 { + 1e-3 + } else { + 1e-2 + }; + perturbed[k] += param_eps; + + let mut row_idx = 0; + for (p_idx, pass) in stage1_passes.iter().enumerate() { + let dt = perturbed[4 + 2 * p_idx]; + let df = perturbed[4 + 2 * p_idx + 1]; + for pt in &pass.points { + let pred = predict_frequency( + perturbed[0], + perturbed[1], + perturbed[2], + perturbed[3], + epoch, + pt.time, + dt, + df, + center_freq, + rec_ecef, + ); + let diff = pt.freq - (center_freq + pred); + jacobian[row_idx][k] = (diff - residuals[row_idx]) / param_eps; + row_idx += 1; + } + } + for p_idx in 0..stage1_passes.len() { + let dt = perturbed[4 + 2 * p_idx]; + let diff = dt * 10.0; jacobian[row_idx][k] = (diff - residuals[row_idx]) / param_eps; row_idx += 1; } } - for p_idx in 0..stage1_passes.len() { - let dt = perturbed[4 + 2 * p_idx]; - let diff = dt * 10.0; - jacobian[row_idx][k] = (diff - residuals[row_idx]) / param_eps; - row_idx += 1; - } - } - let mut jt_j = vec![vec![0.0; 8]; 8]; - let mut jt_r = vec![0.0; 8]; - for row in 0..n_obs { - for c1 in 0..8 { - jt_r[c1] += jacobian[row][c1] * residuals[row]; - for c2 in 0..8 { - jt_j[c1][c2] += jacobian[row][c1] * jacobian[row][c2]; + let mut jt_j = vec![vec![0.0; 8]; 8]; + let mut jt_r = vec![0.0; 8]; + for row in 0..n_obs { + for c1 in 0..8 { + jt_r[c1] += jacobian[row][c1] * residuals[row]; + for c2 in 0..8 { + jt_j[c1][c2] += jacobian[row][c1] * jacobian[row][c2]; + } } } - } - for k in 0..8 { - jt_j[k][k] += stage1_lambda * jt_j[k][k]; - } - - if let Some(delta) = solve_linear_system(&mut jt_j, &jt_r) { for k in 0..8 { - stage1_params[k] -= delta[k]; + jt_j[k][k] += stage1_lambda * jt_j[k][k]; } - stage1_params[0] = stage1_params[0].max(6500e3).min(20000e3); - stage1_params[1] = stage1_params[1].max(0.0).min(std::f64::consts::PI); - // Scale-aware convergence: check relative step size per parameter. - let max_rel_step = (0..8).map(|k| { - let denom = stage1_params[k].abs().max(1e-10); - delta[k].abs() / denom - }).fold(0.0f64, f64::max); - if max_rel_step < 1e-8 { + + if let Some(delta) = solve_linear_system(&mut jt_j, &jt_r) { + for k in 0..8 { + stage1_params[k] -= delta[k]; + } + stage1_params[0] = stage1_params[0].max(6500e3).min(20000e3); + stage1_params[1] = stage1_params[1].max(0.0).min(std::f64::consts::PI); + let max_rel_step = (0..8).map(|k| { + let denom = stage1_params[k].abs().max(1e-10); + delta[k].abs() / denom + }).fold(0.0f64, f64::max); + if max_rel_step < 1e-8 { + break; + } + } else { break; } + } + (best_stage1_params, best_stage1_rss) + }; + + let (stage1_params_init_a, _) = run_stage1_lm(best_init_a_state); + let (stage1_params, _) = if best_a_est_rss < f64::MAX { + let (stage1_params_a_est, _) = run_stage1_lm(best_a_est_state); + let rss_all_init_a = compute_rss( + raw_passes, + rec_ecef, + stage1_params_init_a[0], + stage1_params_init_a[1], + epoch, + center_freq, + stage1_params_init_a[2], + stage1_params_init_a[3], + ); + let rss_all_a_est = compute_rss( + raw_passes, + rec_ecef, + stage1_params_a_est[0], + stage1_params_a_est[1], + epoch, + center_freq, + stage1_params_a_est[2], + stage1_params_a_est[3], + ); + if rss_all_init_a < rss_all_a_est { + (stage1_params_init_a, rss_all_init_a) } else { - break; + (stage1_params_a_est, rss_all_a_est) } - } - stage1_params = best_stage1_params; + } else { + let rss_all_init_a = compute_rss( + raw_passes, + rec_ecef, + stage1_params_init_a[0], + stage1_params_init_a[1], + epoch, + center_freq, + stage1_params_init_a[2], + stage1_params_init_a[3], + ); + (stage1_params_init_a, rss_all_init_a) + }; // --- STAGE 2: Fit using all passes, initialized with Stage 1 refined parameters --- params[0] = stage1_params[0]; @@ -1383,6 +1495,7 @@ pub fn compute_rss( let dt = (pred_pca_time - obs_pca_time).num_milliseconds() as f64 / 1000.0; rss += 100.0 * dt * dt; + let mut diffs = Vec::with_capacity(pass.points.len()); for pt in &pass.points { let pred_f = predict_frequency( initial_a, @@ -1396,8 +1509,16 @@ pub fn compute_rss( center_freq, rec_ecef, ); - let diff = pt.freq - (center_freq + pred_f); - rss += diff * diff; + diffs.push(pt.freq - (center_freq + pred_f)); + } + let mean_df = if !diffs.is_empty() { + diffs.iter().sum::() / diffs.len() as f64 + } else { + 0.0 + }; + for diff in diffs { + let residual = diff - mean_df; + rss += residual * residual; } } rss diff --git a/src/space_weather.rs b/src/space_weather.rs new file mode 100644 index 0000000..b0d665f --- /dev/null +++ b/src/space_weather.rs @@ -0,0 +1,165 @@ +use num_complex::Complex; +use rustfft::FftPlanner; + +/// Compute the intensity scintillation index S4 from a window of signal amplitudes +pub fn compute_s4(amplitudes: &[f64]) -> f64 { + if amplitudes.is_empty() { + return 0.0; + } + let mut sum_i = 0.0; + let mut sum_i2 = 0.0; + for &a in amplitudes { + let i = a * a; + sum_i += i; + sum_i2 += i * i; + } + let n = amplitudes.len() as f64; + let mean_i = sum_i / n; + let mean_i2 = sum_i2 / n; + + if mean_i > 1e-12 { + let variance_i = mean_i2 - mean_i * mean_i; + if variance_i > 0.0 { + (variance_i.sqrt() / mean_i).min(2.0) + } else { + 0.0 + } + } else { + 0.0 + } +} + +/// Compute the phase scintillation index sigma_phi from a window of phase innovations (radians) +pub fn compute_sigma_phi(phase_innovations: &[f64]) -> f64 { + if phase_innovations.is_empty() { + return 0.0; + } + let mut sum_p = 0.0; + let mut sum_p2 = 0.0; + for &p in phase_innovations { + sum_p += p; + sum_p2 += p * p; + } + let n = phase_innovations.len() as f64; + let mean_p = sum_p / n; + let mean_p2 = sum_p2 / n; + let variance_p = mean_p2 - mean_p * mean_p; + if variance_p > 0.0 { + variance_p.sqrt() + } else { + 0.0 + } +} + +/// Perform cepstral analysis on the phase innovations to identify periodic tumbling. +/// Returns the detected tumbling frequency (Hz) and peak prominence/magnitude. +pub fn analyze_attitude(history: &[f64], fs: f64) -> Option<(f64, f64)> { + let n = history.len(); + if n < 256 { + return None; + } + + // Use a power-of-two FFT size <= history length + let mut fft_size = 256; + while fft_size * 2 <= n { + fft_size *= 2; + } + let fft_size = fft_size.min(1024); + if history.len() < fft_size { + return None; + } + + let start_idx = history.len() - fft_size; + let window_data = &history[start_idx..]; + + // Forward FFT + let mut planner = FftPlanner::new(); + let fft = planner.plan_fft_forward(fft_size); + let mut buffer: Vec> = window_data + .iter() + .map(|&x| Complex::new(x, 0.0)) + .collect(); + fft.process(&mut buffer); + + // Compute log magnitude of the spectrum + let mut log_mag: Vec> = buffer + .iter() + .map(|&c| Complex::new((c.norm() + 1e-12).ln(), 0.0)) + .collect(); + + // Inverse FFT (IFFT) to get the Cepstrum + let ifft = planner.plan_fft_inverse(fft_size); + ifft.process(&mut log_mag); + + // Normalize IFFT output + for c in &mut log_mag { + *c = *c / (fft_size as f64); + } + + // Search for the maximum peak in the real part of the Cepstrum, + // excluding the DC and low-quefrency region (below 1.0 second period) + // and up to fft_size / 2 (symmetric) + let min_bin = (fs * 1.0) as usize; + let max_bin = fft_size / 2; + if min_bin >= max_bin { + return None; + } + + let mut best_bin = min_bin; + let mut best_val = -1e9; + for bin in min_bin..max_bin { + let val = log_mag[bin].re; + if val > best_val { + best_val = val; + best_bin = bin; + } + } + + // tumbling frequency = fs / bin + let t_period = (best_bin as f64) / fs; + if t_period > 0.0 { + let freq = 1.0 / t_period; + Some((freq, best_val)) + } else { + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_scintillation_indices() { + let amplitudes = vec![1.0, 1.1, 0.9, 1.0, 1.2, 0.8]; + let s4 = compute_s4(&litudes); + assert!(s4 > 0.0 && s4 < 1.0); + + let phase_errors = vec![0.0, 0.1, -0.1, 0.05, -0.05]; + let sigma = compute_sigma_phi(&phase_errors); + assert!(sigma > 0.0 && sigma < 0.2); + } + + #[test] + fn test_cepstral_attitude_analysis() { + // Generate a 0.2 Hz tumbling modulation sampled at 50 Hz + let fs = 50.0; + let mut history = Vec::new(); + for i in 0..1024 { + let t = (i as f64) / fs; + let mut val = 0.0; + // Sum of 5 harmonics to create a periodic ripple in the spectrum + for k in 1..=5 { + val += (2.0 * std::f64::consts::PI * (0.2 * k as f64) * t).cos(); + } + history.push(val); + } + + let result = analyze_attitude(&history, fs); + assert!(result.is_some()); + let (freq, prominence) = result.unwrap(); + // Freq should be very close to 0.2 Hz + assert!((freq - 0.2).abs() < 0.05); + assert!(prominence > 0.0); + } +} diff --git a/tests/langevin_stress_tests.rs b/tests/langevin_stress_tests.rs index 1934683..1e0c1a3 100644 --- a/tests/langevin_stress_tests.rs +++ b/tests/langevin_stress_tests.rs @@ -2,7 +2,7 @@ use chrono::{DateTime, Datelike, TimeZone, Timelike, Utc}; pub mod orbit { use super::*; - pub fn datetime_to_jd(dt: DateTime) -> f64 { + pub fn datetime_to_jd(dt: DateTime) -> (f64, f64) { let year = dt.year() as f64; let month = dt.month() as f64; let day = dt.day() as f64; @@ -12,7 +12,6 @@ pub mod orbit { let nanosecond = dt.nanosecond() as f64; let day_fraction = (hour + (minute + (second + nanosecond / 1e9) / 60.0) / 60.0) / 24.0; - let jd_day = day + day_fraction; let (y, m) = if month <= 2.0 { (year - 1.0, month + 12.0) @@ -23,11 +22,12 @@ pub mod orbit { let a = (y / 100.0).floor(); let b = 2.0 - a + (a / 4.0).floor(); - (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + jd_day + b - 1524.5 + let jd_base = (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + day + b - 1524.5; + (jd_base, day_fraction) } - pub fn teme_to_ecef(jd: f64, pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { - let d = jd - 2451545.0; + pub fn teme_to_ecef(jd: (f64, f64), pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { + let d = (jd.0 - 2451545.0) + jd.1; let t = d / 36525.0; let mut gmst = 280.46061837 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000.0; @@ -52,6 +52,37 @@ pub mod orbit { ([x_ecef, y_ecef, z_ecef], [vx_ecef, vy_ecef, vz_ecef]) } + pub fn apply_sagnac_correction( + pos_sat: [f64; 3], + vel_sat: [f64; 3], + pos_obs: [f64; 3], + ) -> ([f64; 3], [f64; 3]) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let range = (dx * dx + dy * dy + dz * dz).sqrt(); + if range > 0.0 { + let tau = range / 299792458.0; + let omega_e = 7.2921151467e-5; + let theta_sagnac = -omega_e * tau; + let cos_t = theta_sagnac.cos(); + let sin_t = theta_sagnac.sin(); + let p_corr = [ + pos_sat[0] * cos_t + pos_sat[1] * sin_t, + -pos_sat[0] * sin_t + pos_sat[1] * cos_t, + pos_sat[2], + ]; + let v_corr = [ + vel_sat[0] * cos_t + vel_sat[1] * sin_t, + -vel_sat[0] * sin_t + vel_sat[1] * cos_t, + vel_sat[2], + ]; + (p_corr, v_corr) + } else { + (pos_sat, vel_sat) + } + } + pub fn wgs84_to_ecef(lat_deg: f64, lon_deg: f64, alt_m: f64) -> [f64; 3] { let lat = lat_deg.to_radians(); let lon = lon_deg.to_radians(); diff --git a/tests/verify_audit_remediations.rs b/tests/verify_audit_remediations.rs new file mode 100644 index 0000000..bc3d12a --- /dev/null +++ b/tests/verify_audit_remediations.rs @@ -0,0 +1,126 @@ +#[cfg(any(target_arch = "x86", target_arch = "x86_64"))] +use num_complex::Complex; +use chrono::TimeZone; +use sattime::daemon::{ConsensusSteeringEngine, CompletedPassData}; +use sattime::orbit::{ + datetime_to_jd, apply_sagnac_correction, saastamoinen_tropospheric_delay, + wgs84_to_ecef +}; +#[cfg(any(target_arch = "x86", target_arch = "x86_64"))] +use sattime::dsp::DigitalDownConverter; + +#[test] +fn test_bounded_consensus_engine_history() { + let mut engine = ConsensusSteeringEngine::new(); + assert_eq!(engine.passes.len(), 0); + + for i in 0..60 { + engine.add_pass_result(CompletedPassData { + sat_name: format!("SAT_{}", i), + timestamp: chrono::Utc::now(), + offset_seconds: 0.002, + freq_drift_ppm: 0.01, + snr: 15.0, + max_elevation: 60.0, + fit_rmse: 0.5, + }); + } + + assert_eq!(engine.passes.len(), 50); + // Should contain SAT_10 through SAT_59 (since SAT_0 to SAT_9 were popped) + assert_eq!(engine.passes[0].sat_name, "SAT_10"); + assert_eq!(engine.passes[49].sat_name, "SAT_59"); +} + +#[test] +fn test_two_part_julian_date_precision() { + // 2000-01-01T12:00:00Z is exactly Julian Date 2451545.0 + let dt = chrono::Utc.with_ymd_and_hms(2000, 1, 1, 12, 0, 0).unwrap(); + let jd = datetime_to_jd(dt); + + // Day fraction at noon is 0.5 + assert_eq!(jd.1, 0.5); + // Integer base should be 2451544.5 (since base + fraction = 2451545.0) + assert_eq!(jd.0, 2451544.5); + assert_eq!(jd.0 + jd.1, 2451545.0); + + // 2000-01-01T18:00:00Z should have day fraction 0.75 + let dt2 = chrono::Utc.with_ymd_and_hms(2000, 1, 1, 18, 0, 0).unwrap(); + let jd2 = datetime_to_jd(dt2); + assert_eq!(jd2.1, 0.75); + assert_eq!(jd2.0, 2451544.5); + assert_eq!(jd2.0 + jd2.1, 2451545.25); +} + +#[test] +fn test_sagnac_correction_correctness() { + let pos_sat = [7000e3, 0.0, 0.0]; + let vel_sat = [0.0, 7500.0, 0.0]; + let pos_obs = [6378e3, 0.0, 0.0]; + + // Sat to obs distance: 622 km. Time of flight: ~2.07 ms. + // Sagnac angle should be roughly -1.5e-7 rad. + let (pos_corr, vel_corr) = apply_sagnac_correction(pos_sat, vel_sat, pos_obs); + + // Rotation should affect y (since it rotates around Z) + assert_ne!(pos_corr[0], pos_sat[0]); + assert_ne!(pos_corr[1], pos_sat[1]); + assert_eq!(pos_corr[2], pos_sat[2]); + + assert_ne!(vel_corr[0], vel_sat[0]); + assert_ne!(vel_corr[1], vel_sat[1]); + assert_eq!(vel_corr[2], vel_sat[2]); +} + +#[test] +fn test_saastamoinen_delay_output_values() { + let obs_ecef = wgs84_to_ecef(45.0, -75.0, 100.0); + // Place satellite exactly overhead + let sat_ecef = [obs_ecef[0] * 1.1, obs_ecef[1] * 1.1, obs_ecef[2] * 1.1]; + + let delay = saastamoinen_tropospheric_delay(sat_ecef, obs_ecef); + // At zenith (elevation = 90 deg), delay is roughly 2.3 / (1.0 + 0.00143) = 2.296 meters + assert!((delay - 2.296).abs() < 0.01); +} + +#[test] +fn test_avx2_ddc_equivalence() { + #[cfg(any(target_arch = "x86", target_arch = "x86_64"))] + { + if is_x86_feature_detected!("avx2") && is_x86_feature_detected!("fma") { + let mut ddc_scalar = DigitalDownConverter::new(); + let mut ddc_avx2 = DigitalDownConverter::new(); + + // Generate test input + let mut input = vec![Complex::new(0.0f32, 0.0f32); 1024]; + for i in 0..1024 { + let phi = (i as f32) * 0.05; + input[i] = Complex::new(phi.cos(), phi.sin()); + } + + let mut out_scalar = vec![Complex::new(0.0f32, 0.0f32); 1024]; + let mut out_avx2 = vec![Complex::new(0.0f32, 0.0f32); 1024]; + + let f_shift = 15000.0; + let sample_rate = 2e6; + + // Run process on both + ddc_scalar.process(&input, f_shift, sample_rate, &mut out_scalar); + + // Run avx2 directly + unsafe { + ddc_avx2.process_avx2(&input, f_shift, sample_rate, &mut out_avx2); + } + + // Compare outputs + for i in 0..1024 { + let diff_re = (out_scalar[i].re - out_avx2[i].re).abs(); + let diff_im = (out_scalar[i].im - out_avx2[i].im).abs(); + assert!(diff_re < 1e-4, "Mismatch at index {} re: {} vs {}", i, out_scalar[i].re, out_avx2[i].re); + assert!(diff_im < 1e-4, "Mismatch at index {} im: {} vs {}", i, out_scalar[i].im, out_avx2[i].im); + } + + assert!((ddc_scalar.phase_acc - ddc_avx2.phase_acc).abs() < 1e-4); + } + } +} diff --git a/tests/verify_eca_canceler.rs b/tests/verify_eca_canceler.rs index a0dc5c1..ebdc7d6 100644 --- a/tests/verify_eca_canceler.rs +++ b/tests/verify_eca_canceler.rs @@ -1,5 +1,5 @@ use num_complex::Complex; -use sattime::dsp::EcaCanceler; +use sattime::dsp::{EcaCanceler, clean_ambiguity_map}; #[test] fn test_eca_clutter_suppression() { @@ -25,3 +25,35 @@ fn test_eca_clutter_suppression() { ); } } + +#[test] +fn test_clean_algorithm_omp() { + // Generate a simple 10x10 ambiguity map with a large target and a small target + let mut map = vec![vec![0.0f32; 10]; 10]; + map[3][4] = 100.0; // Large airliner target + map[6][7] = 45.0; // Small drone target + + // Add some sidelobes from airliner using Gaussian spread + for r in 0..10 { + let dr = (r as f32 - 3.0).powi(2); + for c in 0..10 { + let dc = (c as f32 - 4.0).powi(2); + map[r][c] += 100.0 * (-dr/8.0 - dc/8.0).exp(); + } + } + // Set exact peak values again + map[3][4] = 100.0; + map[6][7] = 45.0; + + let components = clean_ambiguity_map(&mut map, 2, 0.8); + + assert_eq!(components.len(), 2); + // First component should be airliner at (3, 4) + assert_eq!(components[0].0, 3); + assert_eq!(components[0].1, 4); + assert!(components[0].2 > 90.0); + + // Second component should be drone at (6, 7) + assert_eq!(components[1].0, 6); + assert_eq!(components[1].1, 7); +} diff --git a/tests/verify_orbit_solver.rs b/tests/verify_orbit_solver.rs index b8d1815..ced8438 100644 --- a/tests/verify_orbit_solver.rs +++ b/tests/verify_orbit_solver.rs @@ -3,7 +3,7 @@ use chrono::{DateTime, Datelike, Timelike, Utc}; // Mock/helper implementations of coordinate functions so orbit_solver compiles pub mod orbit { use super::*; - pub fn datetime_to_jd(dt: DateTime) -> f64 { + pub fn datetime_to_jd(dt: DateTime) -> (f64, f64) { let year = dt.year() as f64; let month = dt.month() as f64; let day = dt.day() as f64; @@ -13,7 +13,6 @@ pub mod orbit { let nanosecond = dt.nanosecond() as f64; let day_fraction = (hour + (minute + (second + nanosecond / 1e9) / 60.0) / 60.0) / 24.0; - let jd_day = day + day_fraction; let (y, m) = if month <= 2.0 { (year - 1.0, month + 12.0) @@ -24,11 +23,12 @@ pub mod orbit { let a = (y / 100.0).floor(); let b = 2.0 - a + (a / 4.0).floor(); - (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + jd_day + b - 1524.5 + let jd_base = (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + day + b - 1524.5; + (jd_base, day_fraction) } - pub fn teme_to_ecef(jd: f64, pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { - let d = jd - 2451545.0; + pub fn teme_to_ecef(jd: (f64, f64), pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { + let d = (jd.0 - 2451545.0) + jd.1; let t = d / 36525.0; let mut gmst = 280.46061837 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000.0; @@ -56,6 +56,37 @@ pub mod orbit { ([x_ecef, y_ecef, z_ecef], [vx_ecef, vy_ecef, vz_ecef]) } + pub fn apply_sagnac_correction( + pos_sat: [f64; 3], + vel_sat: [f64; 3], + pos_obs: [f64; 3], + ) -> ([f64; 3], [f64; 3]) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let range = (dx * dx + dy * dy + dz * dz).sqrt(); + if range > 0.0 { + let tau = range / 299792458.0; + let omega_e = 7.2921151467e-5; + let theta_sagnac = -omega_e * tau; + let cos_t = theta_sagnac.cos(); + let sin_t = theta_sagnac.sin(); + let p_corr = [ + pos_sat[0] * cos_t + pos_sat[1] * sin_t, + -pos_sat[0] * sin_t + pos_sat[1] * cos_t, + pos_sat[2], + ]; + let v_corr = [ + vel_sat[0] * cos_t + vel_sat[1] * sin_t, + -vel_sat[0] * sin_t + vel_sat[1] * cos_t, + vel_sat[2], + ]; + (p_corr, v_corr) + } else { + (pos_sat, vel_sat) + } + } + pub fn wgs84_to_ecef(lat_deg: f64, lon_deg: f64, alt_m: f64) -> [f64; 3] { let lat = lat_deg.to_radians(); let lon = lon_deg.to_radians(); diff --git a/tests/verify_orbit_solver_stress.rs b/tests/verify_orbit_solver_stress.rs index f50b76a..7152f00 100644 --- a/tests/verify_orbit_solver_stress.rs +++ b/tests/verify_orbit_solver_stress.rs @@ -2,7 +2,7 @@ use chrono::{DateTime, Datelike, Timelike, Utc}; pub mod orbit { use super::*; - pub fn datetime_to_jd(dt: DateTime) -> f64 { + pub fn datetime_to_jd(dt: DateTime) -> (f64, f64) { let year = dt.year() as f64; let month = dt.month() as f64; let day = dt.day() as f64; @@ -12,7 +12,6 @@ pub mod orbit { let nanosecond = dt.nanosecond() as f64; let day_fraction = (hour + (minute + (second + nanosecond / 1e9) / 60.0) / 60.0) / 24.0; - let jd_day = day + day_fraction; let (y, m) = if month <= 2.0 { (year - 1.0, month + 12.0) @@ -23,11 +22,12 @@ pub mod orbit { let a = (y / 100.0).floor(); let b = 2.0 - a + (a / 4.0).floor(); - (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + jd_day + b - 1524.5 + let jd_base = (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + day + b - 1524.5; + (jd_base, day_fraction) } - pub fn teme_to_ecef(jd: f64, pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { - let d = jd - 2451545.0; + pub fn teme_to_ecef(jd: (f64, f64), pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { + let d = (jd.0 - 2451545.0) + jd.1; let t = d / 36525.0; let mut gmst = 280.46061837 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000.0; @@ -52,6 +52,37 @@ pub mod orbit { ([x_ecef, y_ecef, z_ecef], [vx_ecef, vy_ecef, vz_ecef]) } + pub fn apply_sagnac_correction( + pos_sat: [f64; 3], + vel_sat: [f64; 3], + pos_obs: [f64; 3], + ) -> ([f64; 3], [f64; 3]) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let range = (dx * dx + dy * dy + dz * dz).sqrt(); + if range > 0.0 { + let tau = range / 299792458.0; + let omega_e = 7.2921151467e-5; + let theta_sagnac = -omega_e * tau; + let cos_t = theta_sagnac.cos(); + let sin_t = theta_sagnac.sin(); + let p_corr = [ + pos_sat[0] * cos_t + pos_sat[1] * sin_t, + -pos_sat[0] * sin_t + pos_sat[1] * cos_t, + pos_sat[2], + ]; + let v_corr = [ + vel_sat[0] * cos_t + vel_sat[1] * sin_t, + -vel_sat[0] * sin_t + vel_sat[1] * cos_t, + vel_sat[2], + ]; + (p_corr, v_corr) + } else { + (pos_sat, vel_sat) + } + } + pub fn wgs84_to_ecef(lat_deg: f64, lon_deg: f64, alt_m: f64) -> [f64; 3] { let lat = lat_deg.to_radians(); let lon = lon_deg.to_radians(); diff --git a/tests/verify_solver_robustness.rs b/tests/verify_solver_robustness.rs index d9f446d..a4a4496 100644 --- a/tests/verify_solver_robustness.rs +++ b/tests/verify_solver_robustness.rs @@ -3,7 +3,7 @@ use chrono::{DateTime, Datelike, Timelike, Utc}; // Mirror the orbital helper modules from verify_orbit_solver.rs so orbit_solver.rs compiles. pub mod orbit { use super::*; - pub fn datetime_to_jd(dt: DateTime) -> f64 { + pub fn datetime_to_jd(dt: DateTime) -> (f64, f64) { let year = dt.year() as f64; let month = dt.month() as f64; let day = dt.day() as f64; @@ -13,7 +13,6 @@ pub mod orbit { let nanosecond = dt.nanosecond() as f64; let day_fraction = (hour + (minute + (second + nanosecond / 1e9) / 60.0) / 60.0) / 24.0; - let jd_day = day + day_fraction; let (y, m) = if month <= 2.0 { (year - 1.0, month + 12.0) @@ -24,11 +23,12 @@ pub mod orbit { let a = (y / 100.0).floor(); let b = 2.0 - a + (a / 4.0).floor(); - (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + jd_day + b - 1524.5 + let jd_base = (365.25 * (y + 4716.0)).floor() + (30.6001 * (m + 1.0)).floor() + day + b - 1524.5; + (jd_base, day_fraction) } - pub fn teme_to_ecef(jd: f64, pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { - let d = jd - 2451545.0; + pub fn teme_to_ecef(jd: (f64, f64), pos_teme: [f64; 3], vel_teme: [f64; 3]) -> ([f64; 3], [f64; 3]) { + let d = (jd.0 - 2451545.0) + jd.1; let t = d / 36525.0; let mut gmst = 280.46061837 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000.0; @@ -53,6 +53,37 @@ pub mod orbit { ([x_ecef, y_ecef, z_ecef], [vx_ecef, vy_ecef, vz_ecef]) } + pub fn apply_sagnac_correction( + pos_sat: [f64; 3], + vel_sat: [f64; 3], + pos_obs: [f64; 3], + ) -> ([f64; 3], [f64; 3]) { + let dx = pos_sat[0] - pos_obs[0]; + let dy = pos_sat[1] - pos_obs[1]; + let dz = pos_sat[2] - pos_obs[2]; + let range = (dx * dx + dy * dy + dz * dz).sqrt(); + if range > 0.0 { + let tau = range / 299792458.0; + let omega_e = 7.2921151467e-5; + let theta_sagnac = -omega_e * tau; + let cos_t = theta_sagnac.cos(); + let sin_t = theta_sagnac.sin(); + let p_corr = [ + pos_sat[0] * cos_t + pos_sat[1] * sin_t, + -pos_sat[0] * sin_t + pos_sat[1] * cos_t, + pos_sat[2], + ]; + let v_corr = [ + vel_sat[0] * cos_t + vel_sat[1] * sin_t, + -vel_sat[0] * sin_t + vel_sat[1] * cos_t, + vel_sat[2], + ]; + (p_corr, v_corr) + } else { + (pos_sat, vel_sat) + } + } + pub fn wgs84_to_ecef(lat_deg: f64, lon_deg: f64, alt_m: f64) -> [f64; 3] { let lat = lat_deg.to_radians(); let lon = lon_deg.to_radians();