Research related code and simulation on inertial navigation