This project implements Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) integration using the Unscented Kalman Filter (UKF), developed as part of the RBE595 Robot Navigation course at Worcester Polytechnic Institute. Two complementary filter architectures are implemented and compared.

---

Why UKF?

The navigation equations for INS/GNSS fusion are highly nonlinear — involving rotation matrices, Earth curvature corrections, and gravity models. The EKF approximates these with Jacobians, which can introduce significant linearization errors. The UKF instead uses sigma points (the Unscented Transform) to propagate the distribution through the true nonlinear function, achieving higher accuracy without requiring analytic derivatives.

---

Two Architectures

Feedforward Architecture (12-state)

The INS propagates the full navigation solution independently. GNSS measurements are fused by the UKF to directly correct the current position, velocity, and orientation estimates. The state vector contains:

• 3D position (latitude, longitude, altitude)
• 3D velocity (North/East/Down)
• Orientation (roll, pitch, yaw)

Feedback Architecture (15-state)

Extends the feedforward design with 3 additional IMU bias states (gyroscope and accelerometer). The UKF estimates sensor biases alongside the navigation state, and the corrections are fed back into the INS mechanization loop to continuously compensate for drift — achieving better long-term accuracy.

---

Earth & Gravity Models

Navigation accuracy depends on precise modeling of the Earth’s geometry and gravity field:

• WGS84 ellipsoidal Earth model — captures the oblate shape of the Earth for accurate latitude/longitude/altitude conversion
• Somigliana gravity model — computes surface gravity as a function of latitude, with vertical corrections for altitude
• Earth rotation compensation — angular velocity of the Earth is subtracted from gyroscope measurements to isolate true body rotation

---

UKF Sigma Point Generation

The Julier symmetric sigma point scheme is used. For an n-dimensional state, 2n+1 sigma points are generated:

\[\mathcal{X}_0 = \hat{x}, \quad \mathcal{X}_i = \hat{x} \pm \sqrt{(n+\lambda)P_i}\]

These are propagated through the nonlinear process model, and the mean and covariance are recovered via weighted summation — capturing higher-order moments of the distribution.

---