Added fixes to the F matrix
(For the case with imu sharing and with omega estimated.)
Omega estimation case still does not work.
Smoother seems to have broken. We do not know why.