Overview

This project implements a nonlinear Kalman filter (UKF) for strapdown inertial navigation using MEMS-grade IMU and GNSS data from the MEMS-Nav dataset.
The system estimates full 3D navigation states in the local-level frame and fuses inertial and satellite measurements to produce robust vehicle trajectories with bounded error.

Strapdown INS Modeling

The navigation state is represented in the local-level frame using latitude, longitude, altitude, NED velocity components, and roll–pitch–yaw attitude angles.
Classical strapdown mechanization equations propagate the attitude matrix, transform IMU specific forces into the navigation frame, and update velocity and position while accounting for Earth rotation, transport rate, and gravity variation.

Attitude, Velocity, and Position Updates

Angular rate measurements from the gyroscopes drive an attitude update that includes body rotation, Earth rotation, and local-level transport rate effects.
Transformed specific force is integrated to update NED velocities, which in turn are integrated on the WGS84 ellipsoid to update latitude, longitude, and altitude over time.

Bias and Noise Modeling

To capture key error sources, the state vector augments navigation states with accelerometer and gyroscope biases modeled as random walks.
Process noise levels are chosen per-state (position, velocity, attitude, and biases) to reflect realistic MEMS-grade sensor characteristics and to stabilize the filter.

Unscented Kalman Filter Design

A nonlinear Kalman filter variant is used to fuse IMU-driven INS propagation with GNSS-derived position measurements.
The UKF generates sigma points over the full state, propagates them through the nonlinear motion model, and updates them using GNSS measurements to obtain a corrected state and covariance.

Measurement Models

GNSS provides latitude, longitude, and altitude measurements along with horizontal and vertical accuracy values that inform the measurement noise covariance.
The filter can operate with both basic position-only measurement models and extended models that leverage additional GNSS-derived quantities such as speed and heading when available.

Loosely Coupled, Closed-Loop Fusion

A loosely coupled architecture treats GNSS outputs as direct measurements of navigation state rather than processing raw satellite signals.
Closed-loop feedback is used: the UKF corrections are fed back into the INS state (including sensor biases), reducing drift and improving short-term dead-reckoning performance between GNSS updates.

Error Metrics and Evaluation

Filter performance is evaluated by comparing estimated trajectories against GNSS baselines using raw state differences and great-circle (Haversine) distance.
Additional metrics such as root-mean-square error over time are used to quantify how well the filter tracks latitude, longitude, and altitude across multiple runs.

Accuracy Targets and Extensions

A key design goal is to achieve navigation errors at or below the reported GNSS accuracy values, demonstrating that the filter properly fuses inertial and satellite data.
Extensions include experimenting with different process/measurement noise settings, richer measurement models, or comparing EKF and UKF performance on the same dataset.