diff --git a/gnss-rtk/src/estimate.rs b/gnss-rtk/src/estimate.rs index 3bd3d4b5f..f4063a01c 100644 --- a/gnss-rtk/src/estimate.rs +++ b/gnss-rtk/src/estimate.rs @@ -1,8 +1,11 @@ use nalgebra::base::{DVector, MatrixXx4, Vector4}; +use nyx_space::cosmic::SPEED_OF_LIGHT; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +use log::trace; + /* * Solver solution estimate * is always expressed as a correction of an 'a priori' position @@ -18,10 +21,12 @@ pub struct SolverEstimate { pub dz: f64, /// Time correction pub dt: f64, - /// Position Dilution of Precision + /// Dilution of Position Precision, horizontal component + pub hdop: f64, + /// Dilution of Position Precision, vertical component + pub vdop: f64, + /// Time Dilution of Precision pub tdop: f64, - /// Time (only) Dilution of Precision - pub pdop: f64, } impl SolverEstimate { @@ -31,19 +36,22 @@ impl SolverEstimate { */ pub fn new(g: MatrixXx4, y: DVector) -> Option { let g_prime = g.transpose(); - let q = (g.clone() * g_prime.clone()).try_inverse()?; - let x = g.pseudo_inverse(1.0E-6).unwrap() * y; - //let x = g_prime.clone() * y; - //let x = q.clone() * x; - let pdop = (q[(1, 1)] + q[(2, 2)] + q[(3, 3)]).sqrt(); - let tdop = q[(4, 4)].sqrt(); + let q = (g_prime.clone() * g.clone()).try_inverse()?; + let x = q * g_prime.clone(); + let x = x * y; + + let hdop = (q[(0, 0)] + q[(1, 1)]).sqrt(); + let vdop = q[(2, 2)].sqrt(); + let tdop = q[(3, 3)].sqrt(); + Some(Self { dx: x[0], dy: x[1], dz: x[2], - dt: x[3], + dt: x[3] / SPEED_OF_LIGHT, + hdop, + vdop, tdop, - pdop, }) } } diff --git a/gnss-rtk/src/lib.rs b/gnss-rtk/src/lib.rs index eac19ebb0..cceee0830 100644 --- a/gnss-rtk/src/lib.rs +++ b/gnss-rtk/src/lib.rs @@ -348,7 +348,7 @@ impl Solver { } // 7: resolve - //trace!("y: {} | g: {}", y, g); + trace!("y: {} | g: {}", y, g); let estimate = SolverEstimate::new(g, y); self.nth_epoch += 1; diff --git a/rinex-cli/src/main.rs b/rinex-cli/src/main.rs index 610d61d6a..e318f83ca 100644 --- a/rinex-cli/src/main.rs +++ b/rinex-cli/src/main.rs @@ -691,7 +691,13 @@ pub fn main() -> Result<(), rinex::Error> { match solver.run(&mut ctx) { Ok((t, estimate)) => { let pos = (estimate.dx, estimate.dy, estimate.dz); - trace!("epoch: {} | position error: {:?} | PDOP {} | clock offset: {} | TDOP {}", t, pos, estimate.pdop, estimate.dt, estimate.tdop); + trace!( + "epoch: {} | position error: {:?} | clock offset: {} | TDOP {}", + t, + pos, + estimate.dt, + estimate.tdop + ); }, Err(SolverError::NoSv(t)) => info!("no SV elected @{}", t), Err(SolverError::LessThan4Sv(t)) => info!("less than 4 SV @{}", t),