Smoothers and Kalman Filters

The scripts in this section demonstrate GPS, odometer zupt aided INS implementations. Each of the example is in fact a complete smoother implementation. However, the forward parts can be used as a standard Kalman filtering solution for aided INS systems.

2 filters Smoother
This is a 2 filter smooter implementation. The backward part uses a backward INS to compute the reverse solution and a information form Kalman filter to compute the ML estimates.
You may use the forward filter as an example of a simple vehicular navigation system with GPS+odometer+Zupt. (The INS is mechanized in geodetic frame with quaternions. The position is updated using Cen. The system model is derived based on PSI-Formulation. The position errors are defined in meters rather than radians.)
Bryson-Frazier smoother
Standard Bryson-Frazier smoother implementation. The forward parth demonstrates an example of a aided INS for vehicles (It is a quaternion based INS which is mechanized in geodetic frame. The position is defined in lat, lon, and height. The error model is derived based on PHI formulation.)
Fixed Lag Smoother
This is a fixed lag smoother based on moving window innovation approach. (That is why, it is somehow close to BF formula). In order to deal with instability problem it uses 3 INSs simultaneously.
Rauch-Tung-Striebel Smoother
An RTS implementation which processes real Sensor data with GPS, Odometer and ZUPT. You may use the forward path as an example for a simple vehicular INS with GPS+Odometer+Zupt.
Weinert Smoother
This smoother is based on complementery space approach of Weinert. This specific example processes only ZUPT instants to self calibrate the IMU data without running any INS. The self-calibrated IMU data can later be processed by any other smoother/filter. This example also derives the new time-varying error model of the generated (self-calibrated) data.