Inertial Navigation Code - 24 State EKF