The nonlinear optimization problem with linear constraints has many applications in engineering fields such as the visual-inertial navigation and localization of an unmanned aerial vehicle maintaining the horizontal flight. In order to solve this practical problem efficiently, this paper constructs a continuation method with the trusty time-stepping scheme for the linearly equality-constrained optimization problem at every sampling time. At every iteration, the new method only solves a system of linear equations other than the traditional optimization method such as the sequential quadratic programming (SQP) method, which needs to solve a quadratic programming subproblem. Consequently, the new method can save much more computational time than SQP. Numerical results show that the new method works well for this problem and its consumed time is about one fifth of that of SQP (the built-in subroutine fmincon.m of the MATLAB2018a environment) or that of the traditional dynamical method (the built-in subroutine ode15s.m of the MATLAB2018a environment). Furthermore, we also give the global convergence analysis of the new method.