--This work introduces an algorithm for state estimation on manifolds within the framework of the Kalman filter . Its primary objective is to provide a methodology enabling the evaluation of the precision of existing Kalman filter variants with arbitrary accuracy on synthetic data, something that, to the best of our knowledge, has not been addressed in prior work. T o this end, we develop a new filter that exhibits favorable numerical properties, thereby correcting the divergences observed in previous Kalman filter variants. In this formulation, the achievable precision is no longer constrained by the small-velocity assumption and is determined solely by sensor noise. In addition, this new filter assumes high precision on the sensors, which, in real scenarios require a detection step that we define heuristically, allowing one to extend this approach to scenarios, using either a 9-axis IMU or a combination of odometry, accelerometer, and pressure sensors. The latter configuration is designed for the reconstruction of trajectories in underwater environments. This work has been submitted to the IEEE for possible publication. The present work is motivated by applications in the routine inspection of large metallic structures, such as ship hulls, in underwater environments. The general scenario involves deploying differential-drive robots equipped with acoustic sensing techniques to inspect ship surfaces. The specific problem addressed in this paper is the localization of the robot.

In this paper, the well-known multiplicative extended Kalman filter (MEKF) is re-investigated for attitude estimation using vector observations. From the Lie group theory, it is shown that the attitude estimation model is group affine and its error state model should be trajectory-independent. Moreover, with such trajectory-independent error state model, the linear Kalman filter is still effective for large initialization errors. However, the measurement model of the traditional MEKF is dependent on the attitude prediction, which is therefore trajectory-dependent. This is also the main reason why the performance of traditional MEKF is degraded for large initialization errors. Through substitution of the attitude prediction related term with the vector observation in body frame, a trajectory-independent measurement model is derived for MEKF. Meanwhile, the MEKFs with reference attitude error definition and with global state formulating on special Euclidean group have also been studied, with main focus on derivation of the trajectory-independent measurement models. Extensive Monte Carlo simulations and field test of attitude estimation implementations demonstrate that the performance of MEKFs can be much improved with trajectory-independent measurement models.