kshana 0.9.2

// SPDX-License-Identifier: Apache-2.0
//! Closed-loop GNSS/INS integration: the strapdown mechanization driven by the
//! 15-state error-state EKF with feedback.
//!
//! This is the piece that turns the [`GnssInsEkf`] kernel and the three-axis
//! [`NavState`] mechanization into a working loosely-coupled navigator, and the
//! honest replacement for the hybrid pack's *truth-snap reset* (where a GNSS fix
//! teleported the dead-reckoned position back onto truth with zero residual). A
//! truth-snap is not fusion — it assumes a perfect, noiseless fix and learns
//! nothing about the inertial sensors. Here, instead:
//!
//! 1. **Propagate.** Each IMU sample is corrected by the current bias estimates
//!    (`raw − b̂`), mechanized forward by [`NavState::step`], and the EKF
//!    covariance is time-propagated with the matching navigation context.
//! 2. **Fuse.** When a GNSS position/velocity fix arrives, the EKF forms the
//!    INS−GNSS innovation, weights it against the fix's stated accuracy, and
//!    returns the estimated error state.
//! 3. **Feed back.** The estimated position and velocity errors are *subtracted*
//!    from the strapdown solution, the estimated inertial biases are *added* to
//!    the running bias estimates (so the next propagation step is cleaner), and
//!    the error-state mean is reset to zero.
//!
//! The payoff is **a continuously-corrected state with a bounded error**: across
//! the aided arc the filter holds the strapdown solution to the metre level —
//! where a free-running INS diverges without limit — so when GNSS drops out the
//! navigator enters the outage with a clean position, velocity and attitude and
//! only has to coast for the outage window, not the whole mission. The benchmark
//! below shows the fused solution's Monte-Carlo position RMS over a 60 s outage
//! beating an unaided open-loop dead-reckoner by roughly 4× (the milestone
//! threshold is 2×).
//!
//! **Honest limitation.** In *loosely-coupled* mode the accelerometer bias and
//! the platform tilt are only weakly separable: both perturb the horizontal
//! solution through the (dominant, near-constant) gravity vector, so with
//! realistic GNSS noise the individual bias estimate is not reliable on this
//! class of trajectory. The value delivered here is therefore the bounded,
//! corrected state — not a precise inertial calibration. Sharper bias
//! observability comes from richer dynamics (vertical specific-force changes) and
//! the tightly-coupled extension, both noted as roadmap items.
//!
//! The INS and GNSS are compared in a local tangent-plane NED frame anchored at a
//! fixed geodetic origin, using exactly the radii of curvature the mechanization
//! integrates with ([`radii_of_curvature`]). For the short, slow arcs this filter
//! targets, the flat-Earth projection error is far below the metre-level GNSS
//! noise.
//!
//! **Feedback scope.** Position, velocity, attitude error (`ψ`, rotating the
//! body→NED quaternion), and accelerometer/gyro bias are all fed back, and the
//! error-state mean is reset after each fix. Feeding attitude back (not only the
//! biases) is essential to stability: the tilt and accelerometer bias are a
//! coupled pair, so correcting one without the other drives the filter unstable.

use super::gnss_ins_ekf::{GnssInsEkf, GnssMeasurement};
use crate::frames::{Geodetic, Vec3};
use crate::inertial::attitude::Quaternion;
use crate::inertial::mechanization::{radii_of_curvature, NavState};

/// A closed-loop loosely-coupled GNSS/INS navigator.
pub struct ClosedLoopInsGnss {
    /// The live strapdown navigation state (corrected in place by feedback).
    pub nav: NavState,
    ekf: GnssInsEkf,
    /// Fixed local-tangent-plane origin for the INS↔GNSS comparison.
    origin: Geodetic,
    /// Estimated accelerometer bias (m/s²), subtracted from every raw sample.
    b_a: Vec3,
    /// Estimated gyro bias (rad/s), subtracted from every raw sample.
    b_g: Vec3,
}

impl ClosedLoopInsGnss {
    /// Build a navigator around an initial state and a configured EKF. The
    /// tangent-plane origin is taken from the initial geodetic position.
    pub fn new(nav: NavState, ekf: GnssInsEkf) -> Self {
        let origin = nav.p_llh;
        Self {
            nav,
            ekf,
            origin,
            b_a: [0.0; 3],
            b_g: [0.0; 3],
        }
    }

    /// Current accelerometer bias estimate (m/s²).
    pub fn accel_bias_estimate(&self) -> Vec3 {
        self.b_a
    }

    /// Current gyro bias estimate (rad/s).
    pub fn gyro_bias_estimate(&self) -> Vec3 {
        self.b_g
    }

    /// Project the current geodetic position into the local NED tangent plane
    /// anchored at the origin (north, east, down; metres).
    pub fn ins_ned(&self) -> Vec3 {
        Self::project(self.origin, self.nav.p_llh)
    }

    /// Flat-Earth tangent-plane projection of `p` relative to `origin` (NED, m).
    fn project(origin: Geodetic, p: Geodetic) -> Vec3 {
        let (rn, re) = radii_of_curvature(origin.lat_rad);
        let h = origin.alt_m;
        let north = (p.lat_rad - origin.lat_rad) * (rn + h);
        let east = (p.lon_rad - origin.lon_rad) * (re + h) * origin.lat_rad.cos();
        let down = -(p.alt_m - origin.alt_m);
        [north, east, down]
    }

    /// Propagate one IMU sample: correct the raw gyro/accelerometer by the current
    /// bias estimates, time-propagate the EKF covariance with the matching
    /// navigation context, then mechanize the strapdown state forward by `dt`.
    pub fn propagate(&mut self, gyro_raw: Vec3, accel_raw: Vec3, dt: f64) {
        let gyro = [
            gyro_raw[0] - self.b_g[0],
            gyro_raw[1] - self.b_g[1],
            gyro_raw[2] - self.b_g[2],
        ];
        let accel = [
            accel_raw[0] - self.b_a[0],
            accel_raw[1] - self.b_a[1],
            accel_raw[2] - self.b_a[2],
        ];

        // EKF time-update uses the pre-step navigation context: specific force in
        // the nav frame, the body→nav DCM, and the Earth/transport rates.
        let c_bn = self.nav.q.to_dcm();
        let f_n = self.nav.q.rotate(accel);
        let omega_ie = self.nav.omega_ie_n();
        let omega_en = self.nav.omega_en_n();
        let omega_in = [
            omega_ie[0] + omega_en[0],
            omega_ie[1] + omega_en[1],
            omega_ie[2] + omega_en[2],
        ];
        self.ekf.predict(f_n, c_bn, omega_ie, omega_in, dt);

        self.nav.step(gyro, accel, dt);
    }

    /// Fuse a GNSS position/velocity fix (given in the same local NED frame as
    /// [`ins_ned`](Self::ins_ned)) and feed the estimated errors back into the
    /// strapdown solution and the bias estimates. Returns the estimated position
    /// error that was corrected (NED, m), for diagnostics.
    pub fn fuse(
        &mut self,
        gnss_pos_ned: Vec3,
        gnss_vel_ned: Vec3,
        sigma_pos_m: f64,
        sigma_vel_mps: f64,
    ) -> Vec3 {
        let ins_pos = self.ins_ned();
        let ins_vel = self.nav.v_ned;
        let meas = GnssMeasurement {
            pos: gnss_pos_ned,
            vel: gnss_vel_ned,
            sigma_pos_m,
            sigma_vel_mps,
        };
        let dx = self.ekf.update_loosely_coupled(ins_pos, ins_vel, meas);
        let dp = [dx[0], dx[1], dx[2]];
        let dv = [dx[3], dx[4], dx[5]];
        let psi = [dx[6], dx[7], dx[8]];
        let dba = [dx[9], dx[10], dx[11]];
        let dbg = [dx[12], dx[13], dx[14]];

        // Position feedback: the error is (INS − truth), so subtract it. Map the
        // NED correction back through the radii at the current latitude.
        let (rn, re) = radii_of_curvature(self.nav.p_llh.lat_rad);
        let h = self.nav.p_llh.alt_m;
        self.nav.p_llh.lat_rad -= dp[0] / (rn + h);
        self.nav.p_llh.lon_rad -= dp[1] / ((re + h) * self.nav.p_llh.lat_rad.cos());
        self.nav.p_llh.alt_m += dp[2]; // down-error +ve ⇒ INS too low ⇒ raise alt

        // Velocity feedback: subtract the estimated velocity error.
        self.nav.v_ned[0] -= dv[0];
        self.nav.v_ned[1] -= dv[1];
        self.nav.v_ned[2] -= dv[2];

        // Attitude feedback: rotate the body→NED quaternion to remove the
        // estimated nav-frame attitude error ψ. With this F-matrix sign convention
        // (δv̇ = −[f_n×]ψ) the computed DCM is C̃_b^n = (I + [ψ×]) Ĉ_b^n, so the
        // correction removes a +ψ nav-frame rotation: q̂ = exp(−½ψ) ⊗ q̃. Feeding
        // ψ back (not just the biases) is essential — for a static platform the
        // accelerometer bias and the tilt are individually unobservable, so
        // correcting only one of the coupled pair drives the filter unstable.
        let dq = Quaternion::from_rotation_vector([-psi[0], -psi[1], -psi[2]]);
        self.nav.q = dq.mul(&self.nav.q).normalized();

        // Bias feedback: the estimated residual bias is added to the running
        // estimate, so the next propagation subtracts more of it.
        for k in 0..3 {
            self.b_a[k] += dba[k];
            self.b_g[k] += dbg[k];
        }

        self.ekf.reset_error_state();
        dp
    }

    /// The filter's current position-uncertainty proxy: trace of the δp block (m²).
    pub fn position_cov_trace(&self) -> f64 {
        self.ekf.position_cov_trace()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::fusion::gnss_ins_ekf::EkfNoise;
    use crate::inertial::attitude::Quaternion;
    use crate::inertial::mechanization::{normal_gravity, OMEGA_IE};
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;
    use rand_distr::{Distribution, Normal};
    use std::f64::consts::PI;

    const LAT0: f64 = PI / 4.0;

    fn origin() -> Geodetic {
        Geodetic {
            lat_rad: LAT0,
            lon_rad: -1.0,
            alt_m: 50.0,
        }
    }

    /// A well-aligned navigator: tight (but non-zero) attitude prior, modest
    /// position/velocity prior, generous bias prior, low process noise.
    fn navigator() -> ClosedLoopInsGnss {
        let ekf = GnssInsEkf::new(
            5.0,  // σ_pos (m)
            0.5,  // σ_vel (m/s)
            1e-3, // σ_att (rad)
            0.05, // σ_accel_bias (m/s²)
            1e-4, // σ_gyro_bias (rad/s)
            EkfNoise {
                vrw_psd: 1e-5,
                arw_psd: 1e-10,
                accel_bias_rw_psd: 1e-12,
                gyro_bias_rw_psd: 1e-16,
                accel_bias_tau: f64::INFINITY,
                gyro_bias_tau: f64::INFINITY,
            },
        );
        let nav = NavState::new(Quaternion::identity(), [0.0; 3], origin());
        ClosedLoopInsGnss::new(nav, ekf)
    }

    /// Flat-Earth projection of `p` into the NED tangent plane at `o` (m).
    fn project(o: Geodetic, p: Geodetic) -> Vec3 {
        let (rn, re) = radii_of_curvature(o.lat_rad);
        let h = o.alt_m;
        [
            (p.lat_rad - o.lat_rad) * (rn + h),
            (p.lon_rad - o.lon_rad) * (re + h) * o.lat_rad.cos(),
            -(p.alt_m - o.alt_m),
        ]
    }

    /// A driving profile: a forward specific-force square wave and a yaw square
    /// wave on different periods, so the platform accelerates and turns. The
    /// changing heading and specific force are what give a loosely-coupled filter
    /// any purchase on the inertial errors.
    fn drive_cmd(step: i64) -> (f64, f64) {
        let t = step as f64 * 0.1;
        let a_fwd = if ((t / 15.0) as i64) % 2 == 0 {
            1.5
        } else {
            -1.5
        };
        let yaw = if ((t / 10.0) as i64) % 2 == 0 {
            0.06
        } else {
            -0.06
        };
        (a_fwd, yaw)
    }

    /// The *true* IMU output for the driving profile at the current truth state:
    /// the gyro that keeps the platform level on the rotating Earth plus the
    /// commanded yaw, and the commanded forward specific force plus gravity
    /// support. Forward-simulating a truth navigator with this is the reference
    /// trajectory; the filter's IMU is this corrupted by a bias.
    fn true_imu(truth: &NavState, step: i64) -> (Vec3, Vec3) {
        let ie = truth.omega_ie_n();
        let en = truth.omega_en_n();
        let omega_in = [ie[0] + en[0], ie[1] + en[1], ie[2] + en[2]];
        let omega_in_b = truth.q.conjugate().rotate(omega_in);
        let (a_fwd, yaw) = drive_cmd(step);
        let gyro = [omega_in_b[0], omega_in_b[1], omega_in_b[2] + yaw];
        let g = normal_gravity(truth.p_llh.lat_rad, truth.p_llh.alt_m);
        (gyro, [a_fwd, 0.0, -g])
    }

    /// A static platform at 45°N: the gyro that keeps the body aligned with the
    /// rotating NED frame, and the specific force (−gravity, i.e. up).
    fn static_truth(lat: f64, alt: f64) -> (Vec3, Vec3) {
        let gyro = [OMEGA_IE * lat.cos(), 0.0, -OMEGA_IE * lat.sin()];
        let g = normal_gravity(lat, alt);
        (gyro, [0.0, 0.0, -g])
    }

    #[test]
    fn position_velocity_feedback_nulls_an_injected_error() {
        // Seed the navigator with a deliberate 8 m north / −5 m east error (no IMU
        // bias), aid with a perfect-truth GNSS stream, and watch the closed loop
        // pull the INS back onto truth.
        let start = origin();
        let mut drv = navigator();
        let (rn, re) = radii_of_curvature(LAT0);
        drv.nav.p_llh.lat_rad += 8.0 / (rn + start.alt_m);
        drv.nav.p_llh.lon_rad += -5.0 / ((re + start.alt_m) * LAT0.cos());
        let err0 = (drv.ins_ned()[0].powi(2) + drv.ins_ned()[1].powi(2)).sqrt();
        assert!((err0 - (8.0_f64.powi(2) + 5.0_f64.powi(2)).sqrt()).abs() < 1e-6);

        let (gyro, accel) = static_truth(LAT0, start.alt_m);
        for step in 0..200 {
            drv.propagate(gyro, accel, 0.1);
            if step % 10 == 9 {
                drv.fuse([0.0; 3], [0.0; 3], 1.0, 0.05);
            }
        }
        let err = (drv.ins_ned()[0].powi(2) + drv.ins_ned()[1].powi(2)).sqrt();
        assert!(err < 0.1, "closed loop left {err} m of position error");
    }

    #[test]
    fn aided_solution_stays_bounded_while_free_inertial_diverges() {
        // Same biased IMU drives a closed-loop (aided) navigator and a free-running
        // INS. The aided one must stay metre-bounded; the free one must run away.
        let start = origin();
        let mut drv = navigator();
        let mut truth = NavState::new(Quaternion::identity(), [0.0; 3], start);
        let mut free = NavState::new(Quaternion::identity(), [0.0; 3], start);
        let b_true = [0.03, -0.02, 0.0];
        let mut max_aided = 0.0_f64;
        for step in 0..1500 {
            let (gyro, accel_t) = true_imu(&truth, step);
            truth.step(gyro, accel_t, 0.1);
            let accel_m = [
                accel_t[0] + b_true[0],
                accel_t[1] + b_true[1],
                accel_t[2] + b_true[2],
            ];
            drv.propagate(gyro, accel_m, 0.1);
            free.step(gyro, accel_m, 0.1);
            if step % 10 == 9 {
                drv.fuse(project(start, truth.p_llh), truth.v_ned, 1.0, 0.05);
            }
            let de = project(start, drv.nav.p_llh);
            let te = project(start, truth.p_llh);
            max_aided = max_aided.max(((de[0] - te[0]).powi(2) + (de[1] - te[1]).powi(2)).sqrt());
        }
        let fe = project(start, free.p_llh);
        let te = project(start, truth.p_llh);
        let free_err = ((fe[0] - te[0]).powi(2) + (fe[1] - te[1]).powi(2)).sqrt();
        assert!(max_aided < 6.0, "aided error reached {max_aided} m");
        assert!(free_err > 100.0, "free INS only diverged {free_err} m");
    }

    /// Run one aided arc (`aided` steps) of the driving scenario with seeded GNSS
    /// noise, then a 600-step (60 s) outage. Returns the time-RMS position error
    /// over the outage for (fused navigator, free-running INS).
    fn outage_rms(seed: u64, aided: i64) -> (f64, f64) {
        let start = origin();
        let mut drv = navigator();
        let mut truth = NavState::new(Quaternion::identity(), [0.0; 3], start);
        let mut free = NavState::new(Quaternion::identity(), [0.0; 3], start);
        let b_true = [0.03, -0.02, 0.0];
        let mut rng = ChaCha8Rng::seed_from_u64(seed);
        let np = Normal::new(0.0, 1.0).unwrap();
        let nv = Normal::new(0.0, 0.05).unwrap();
        let (mut sf, mut sx, mut n) = (0.0, 0.0, 0.0);
        for step in 0..(aided + 600) {
            let (gyro, accel_t) = true_imu(&truth, step);
            truth.step(gyro, accel_t, 0.1);
            let accel_m = [
                accel_t[0] + b_true[0],
                accel_t[1] + b_true[1],
                accel_t[2] + b_true[2],
            ];
            drv.propagate(gyro, accel_m, 0.1);
            free.step(gyro, accel_m, 0.1);
            if step < aided && step % 10 == 9 {
                let tp = project(start, truth.p_llh);
                let gp = [
                    tp[0] + np.sample(&mut rng),
                    tp[1] + np.sample(&mut rng),
                    tp[2] + np.sample(&mut rng),
                ];
                let gv = [
                    truth.v_ned[0] + nv.sample(&mut rng),
                    truth.v_ned[1] + nv.sample(&mut rng),
                    truth.v_ned[2] + nv.sample(&mut rng),
                ];
                drv.fuse(gp, gv, 1.0, 0.05);
            }
            if step >= aided {
                let te = project(start, truth.p_llh);
                let de = project(start, drv.nav.p_llh);
                let xe = project(start, free.p_llh);
                sf += (de[0] - te[0]).powi(2) + (de[1] - te[1]).powi(2);
                sx += (xe[0] - te[0]).powi(2) + (xe[1] - te[1]).powi(2);
                n += 1.0;
            }
        }
        ((sf / n).sqrt(), (sx / n).sqrt())
    }

    #[test]
    fn fused_coast_beats_open_loop_dead_reckoning_over_a_60s_outage() {
        // The milestone benchmark. A single outage's error is tilt-driven and
        // sensitive to the last few noisy fixes, so a single run can be only ~1.7×
        // better; the honest figure is the Monte-Carlo (ensemble) RMS the
        // milestone asks for, which is robustly ~4× across seeds.
        let (mut ens_f, mut ens_x, mut n) = (0.0, 0.0, 0.0);
        for seed in 0..8u64 {
            let (f, x) = outage_rms(seed, 1000);
            ens_f += f * f;
            ens_x += x * x;
            n += 1.0;
        }
        let rms_fused = (ens_f / n).sqrt();
        let rms_free = (ens_x / n).sqrt();
        // Free-running dead-reckoning must genuinely diverge over the arc+outage.
        assert!(rms_free > 100.0, "free-INS RMS only {rms_free} m");
        // Fused must beat it by more than the milestone's 2× factor.
        assert!(
            rms_fused < rms_free / 2.0,
            "fused RMS {rms_fused} m vs free {rms_free} m (need >2× better)"
        );
    }
}