In my mind, the gravity can be calculated by rotation from body to world and [0, 0, 9.8], what is the advantage of putting the gravity into the state vector?
I found it converge very slowly, is there any methods for enhancing the convergence rate?
In the world-centric formulation, the gravity in global frame is usually treated as a constant vector [0,0,9.8] because the estimation also runs in the same frame. While, in the robocentric formulation the estimation now runs in a robot (local) frame, and thus the gravity in local frame is not constant and we do not know its exact value. If you only rotate the global gravity to local, the uncertainty of local gravity itself has been ignored. So the first reason to put the gravity into the state vector is to take into account this part of uncertainty. The second and the most important reason is that the robocentric VINS formulation achieves invariant observability properties after adding the gravity into the state vector, that, in addition to the first reason, further improves the estimation consistency. Please refer to our paper for the details.
In the code, the gravity vector is treated as a unit vector, because its magnitude is known as 9.8. Also, I re-normalize the updated gravity vector to make sure that its norm is 1. So, there should be no convergence issue about it.