Added some checks on the coordinate transform and the code to estimate the VI orientation from the gravity vector. Gravity seems good, but it still doesn't converge.. Position plots now show the input data along with the output of the measurement model given the current states