InertialNav : Inertial Nav filter - first attempt