EKF on Lie Groups for Autonomous Underwater Vehicles orientation initialization in presence of magnetic disturbances

Orientation estimation is a fundamental aspect of navigation and motion control of Autonomous Underwater Vehicles (AUVs). This concept is especially true when position sensors are unavailable and, consequently, navigation and control rely on dead reckoning strategies; in this case, orientation estimation is used in conjunction with speed measurements to update position estimation. When unknown magnetic disturbances are present, the magnetometers of the Inertial Measurement Unit (IMU) are unusable and do not provide an accurate initialization of the vehicle heading angle. This issue can be faced by applying a generalization of the Extended Kalman Filter in which the system state and measurements evolve on matrix Lie groups. The filter is used when the AUV is moving on the sea surface and it provides an estimate of the heading offset by comparing the speed measurements acquired by the Global Positioning System (GPS) and the Doppler Velocity Log (DVL) and by fusing the data coming from the IMU and the Fiber Optic Gyroscope (FOG). The initialization procedure has been validated with a dataset acquired by FeelHippo AUV in Cecina, Italy (September 2021).