Strapdown-INS-Cpp

Header-only C++ library implementing a full Strapdown Inertial Navigation System (INS) — from initial alignment through continuous mechanization to theoretical performance analysis via the Van Loan method.

Mathematical Architecture

1. Initial Alignment (InitialAlignment)

Estimates the initial body-to-navigation DCM $C_b^n$ from static IMU observations.

Gravity Leveling: $$\phi = \arctan_2(-f_y, -f_z), \qquad \theta = \arctan\frac{f_x}{\sqrt{f_y^2 + f_z^2}}$$

Gyrocompassing: Heading $\psi$ is extracted by projecting Earth's rotation vector into the leveled frame.

2. Strapdown Mechanization (StrapdownNavigator)

Propagates attitude, velocity, and position epoch-by-epoch in the NED navigation frame.

Attitude Update (Rodrigues): $$C_{b(k+1)}^n = C_{b(k)}^n \left[ I + \frac{\sin|\Delta\theta|}{|\Delta\theta|}[\Delta\theta\times] + \frac{1-\cos|\Delta\theta|}{|\Delta\theta|^2}[\Delta\theta\times]^2 \right]$$

Velocity Update (Trapezoidal, Coriolis-compensated): $$v_{k+1}^n = v_k^n + \frac{1}{2}(a_k^n + a_{k+1}^n)\Delta t, \qquad a^n = C_b^n f^b - (2\omega_{ie}^n + \omega_{en}^n) \times v^n + g^n$$

Transport Rate: $$\omega_{en}^n = \left[\frac{v_E}{R}, ; -\frac{v_N}{R}, ; -\frac{v_E \tan\varphi}{R}\right]^T$$

3. Error Model & Van Loan (ErrorModel)

9-state linearized error dynamics: $\delta\mathbf{x} = [\delta x, \delta v, \delta\psi]^T$

The Van Loan method converts continuous-time system matrices $(A, B, Q_u)$ to discrete transition and process noise matrices $(\Phi_k, Q_k)$ via a single $18\times18$ matrix exponential — avoiding numerical integration entirely.

Build

Requires CMake ≥ 3.14 and a C++17 compiler. Eigen is downloaded automatically.

git clone https://github.com/AmrFawzy-NavEng/Strapdown-INS-Cpp.git
cd Strapdown-INS-Cpp
mkdir build && cd build
cmake ..
cmake --build .

Run

./strapdown_demo              # Runs the full INS pipeline, outputs CSV
cd .. && python scripts/plot_results.py   # Generates plots from CSV

Example Output

The demo generates a synthetic 60-second vehicle trajectory (accelerate → cruise → turn → brake) and processes it through the complete INS pipeline.

Project Structure

Strapdown-INS-Cpp/
├── CMakeLists.txt                    # Build system (auto-fetches Eigen)
├── include/strapdown_ins/
│   ├── types.h                       # Eigen type aliases, ImuSample, NavState
│   ├── rotation.h                    # Skew-symmetric, Rodrigues, DCM↔Euler
│   ├── alignment.h                   # InitialAlignment (leveling + gyrocompassing)
│   ├── navigator.h                   # StrapdownNavigator (full mechanization)
│   └── error_model.h                 # 9-state Van Loan error propagation
├── examples/
│   └── strapdown_demo.cpp            # Self-contained demo with synthetic IMU
└── scripts/
    └── plot_results.py               # Visualization from CSV output

License

MIT — see LICENSE for details.