Added some MSCKF math