[TASK] UKF #745

Description of task

Description of task

Implement an Unscented Kalman Filter (UKF) for AUV navigation as an alternative to the ESKF. The UKF should use quaternion formulation of orientation in order to avoid problems downstream with gimbal lock. The nominal state should be 16D (position, velocity, attitude error, accelerometer bias, gyro bias), however in the unscented transform itself it is necessary to use a 15D covariance matrix as using the full 16D matrix would yield singularities due to the nature of the SO(3) manifold Attitude must be handled on the manifold. I.e we will generate sigma points in the 3-dimensional attitude error space and map them onto the nominal quaternion. The UKF should also share the same sensor models (IMU prediction, DVL and depth updates) and the same ROS interface, so the two filters can be swapped and compared directly. With the exception of the magnetometer which will be implemented in the UKF to mitigate the weakly observable heading.

Contacts

assignee @Q3rkses

My navigation guru - @chrstrom
Testing and quality control expert - @Andeshog
A once relevant vortexer - @Talhanc
My GOAT - @ApatShe
Control team lead - @anbit-adhi
some guy i worked with @jorgenfj

Suggested Workflow

1. Read through the ESKF in PR #640: its state definition, IMU/DVL/depth models, NIS gating and ROS interface. Reuse as much of it as possible.
2. Read up on the theory:
   • The scaled unscented transform (Wan & van der Merwe, 2000).
   • UKFs on manifolds: Crassidis & Markley, 2003 (USQUE); Brossard, Barrau & Bonnabel, 2020 (UKF-M).
   • Solà, 2017, Quaternion kinematics for the error-state Kalman filter, for the shared error-state conventions.
3. Implement the sigma-point generation and unscented transform as a standalone, unit-tested component.
4. Implement IMU prediction and DVL/depth updates, with the same NIS gating as the ESKF.
5. Run both filters on the same simulator runs and rosbags. Compare RMSE, NEES/NIS consistency and runtime, and summarise the results on the wiki.

Specifications

1. Must handle DVL dropout (rolls, near-surface) without corrupting the estimate or jumping on recovery.
2. Covariance must stay symmetric positive definite. The Cholesky decomposition used for sigma points must not fail silently; handle a failed factorisation explicitly.
3. Must run in real time at the IMU rate on the onboard computer.
4. Must publish the same output messages as the ESKF and be selectable through a launch parameter.
5. The filter must be consistent in simulation: NEES stays within its χ² bounds.

Code Quality

• The code is relatively easy to reason about for an outside person when the theory is understood
• Every function in the header files is documented (inputs/returns/exceptions)
• The project has automated tests that cover most of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)