FCC-Dynamics-C

Appearance

Integrated Navigation & SINS Algorithm ​

Status: Implementation In Progress (2026-04-10) Context: High-fidelity Inertial Navigation System (SINS) for FCC.

1. Multi-Mode Navigation Architecture ​

To support development stages, we implement a Dual-Mode Data Routing pattern:

  • Theory Trajectory Mode (NAV_MODE_TRUTH): Directly uses noise-free TruthState for guidance and control prototyping.
  • SINS Navigation Mode (NAV_MODE_SINS): Uses IMU/GPS inputs through the full SINS/Integrated Navigation filter.

Implementation: The RWS Monad routes data based on FccEnv.nav_mode, ensuring switching is "configuration-driven" rather than "code-driven."

2. Coordinate System: NUE (North-Up-East) ​

The navigation frame ($n$-frame) is defined as North-Up-East (NUE).

  • Euler Angle Order: 3-2-1 (Yaw $\psi$, Pitch $\theta$, Roll $\phi$).
  • Strong Typing: All vectors are wrapped in Vec3_T<Frame> (e.g., Vec3_T<BODY>, Vec3_T<NUE>, Vec3_T<LLA>) to prevent cross-frame calculation errors at compile-time.

3. SINS Core Algorithms ​

The Strapdown Inertial Navigation System (SINS) follows the standard update cycle (10ms):

3.1 Attitude Update: Dual-Sample Coning Compensation ​

To handle high-frequency rotational vibrations, we use the Two-Sample Coning Compensation: $$ \Delta\Phi = \Delta\Theta_1 + \Delta\Theta_2 + \frac{2}{3} (\Delta\Theta_1 \times \Delta\Theta_2) $$ Where $\Delta\Theta_i$ are the gyro increments. This minimizes rectification errors.

3.2 Velocity Update: Sculling & Coriolis Compensation ​

Velocity update includes:

  • Specific Force Integration: Body-frame increments transformed to $n$-frame.
  • Sculling Compensation: $\Delta v_{scul} = \frac{1}{2}(\Delta\Theta \times \Delta v) + \dots$
  • Gravity & Coriolis: Compensating for Earth rotation ($\omega_{ie}$) and vehicle transport rate ($\omega_{en}$).

3.3 Position Update ​

Position is updated in the LLA (Latitude, Longitude, Altitude) frame, accounting for Earth's curvature (WGS-84 ellipsoid parameters).

4. Alignment & Initialization ​

  • Static Coarse Alignment: Before liftoff, the system uses "Gravity for Up" and "Earth Rotation for North/East" to estimate the initial attitude matrix $C_b^n$.
  • Ground Alignment Logic: SINS state is initialized when PHYSICS_LIFTOFF is detected, or based on the simulation's ground-lock flag.

5. Integration Filter (GPS/SINS) ​

The architecture supports a standard Error-State Kalman Filter (ESKF):

  • Prediction: SINS Mechanization (High frequency).
  • Update: GPS (Low frequency) correction for position and velocity errors, which are then "reset" into the SINS state.

6. Implementation Notes ​

  • File: src/fcc/navigation/Navigation.cpp
  • Pattern: Functional RWS. The navigation update is a pure function mapping (PrevState, ImuMsg) -> NextState.
  • Safety: Vec3_T ensures that we cannot accidentally add a BODY-frame velocity to an NUE-frame velocity.