Simultaneous localization and mapping (SLAM), which is involved in the inertial navigation and robotic vision, is a hot research topic in some engineering fields for which there are many applications such as unmanned aerial vehicles, autonomous vehicles, remote medical operations and so on (see Corke2016 ; LM2013 ; QLS2018 ; SMP2017 ). In the front end of SLAM, in order to track the pose of camera which is fixed to the vehicle, it needs to solve a matrix differential equation arising from the inertial navigation problem. In engineering fields, they usually adopt the explicit Runge-Kutta method for this differential equation owing to its simplicity of implementation (see Corke2016 ; QLS2018 ).
It is well-known that the explicit Runge-Kutta method is not suitable for a Hamiltonian dynamical system, since the non-symplectic Runge-Kutta method can not preserve its geometric and algebraic properties such as the symplecticity and the orthogonality FQ2010 ; GSS2013 ; HLW2006 ; Higham1997 ; LR2004 ; Sun1993 ; Sun2000 ; SGLS2018 ; WX2013 ; XT2004 . Hong, Liu and Sun HLS2005
consider the symplecticity of a Hamiltonian system which is represented by a PDEs with the skew-symmetric matrix coefficient.
For the inertial navigation problems, the variables are a matrix. We known that the dimension of variables is odd. Thus, it can not directly apply the classical results of the symplectic geometric algorithm to this problem, since the classical symplectic geometric algorithm is applicable to the even dimensional Hamiltonian system. On the other hand, it is important to preserve the geometric and algebraic properties of the differential equation for the numerical method. Therefore, in this article, we investigate some geometric and algebraic properties of the matrix differential equation such as the law of generalized energy, the pseudo-symplecticity and the orthogonal invariant. Consequently, we discuss the geometric and algebraic properties of the symplectic Rune-Kutta method for the linear matrix differential equation with the skew-symmetric matrix coefficient in Section 3. In Section 4, we compare the symplectic Runge-Kutta method with the non-symplectic Runge-Kutta for the differential equation with the skew-symmetric matrix coefficient. The simulation results illustrate the theoretical results of this article. Finally, in Section 5, we give some conclusions and discuss the future work.
2 Geometric Structure of Linear Matrix Differential Equations
We choose a moving coordinate system connected to the aerial vehicle and consider motions of the aerial vehicle where the origin is fixed. By one of Euler’s famous theorems, any such motion is infinitesimally a rotation around an axis. We represent the rotational axis of the aerial vehicle by the direction of a vectorand the speed of rotation by the length of . Thus, the velocity of a mass point of the aerial vehicle is given by the exterior product
which is orthogonal to , orthogonal to , and of length .
We also regard the motion of the aerial vehicle from a coordinate system stationary in the space. The transformation of a vector in the aerial vehicle frame, to the corresponding in the stationary frame, is denoted by
Matrix is orthogonal and describes the rotation of the aerial vehicle. For in the aerial vehicle frame, we find that the columns of matrix are the coordinates of the aerial vehicle’s principal axes in the stationary frame. From equation (2), we know that these rotate with the velocity
Thus, we obtain , which is the rotational velocity expressed in the stationary frame, by the back transformation (2):
where is a skew-symmetric matrix, namely , and represents the rotational transform matrix (see p. 48 in Corke2016 , p. 122 in MP2017 , or p. 278 in HLW2006 ). If we denote and , from equation (4), we have
where is a skew-symmetric matrix. We denote the solution of linear differential equation (5) as
and term the map as the flow map of the given system (5).
We introduce some concepts before we investigate geometric structures of equation (5). Assume that is a smooth function. Its directional derivative along a matrix is denoted here by
where the partial derivatives of are computed at a fixed location , and are the elements of matrix and matrix , respectively. The linear function is called the differential of at and is an example of a differential one-form.
Using this denotation, we define the wedge product of two differentials and as follows (see pp. 61-64 in LR2004 ):
where . Thus, we give the definition of the wedge product of two matrix functions and as follows (see p. 30, Tao2006 ):
where and are the elements of matrix and , respectively. For the wedge product (9) of two matrix differentials and , it also has some basic properties similar to the wedge product of two vector differentials and . We state them as the following Property 2.1.
Let be -matrices of differential one-forms on , then the following properties hold.
Bilinearity: for any ,
Rule of matrix multiplication
for any matrix .
Now we give the pseudo-symplectic property of equation (5).
which proves the result of equation (13).
represents the eigenvalue of matrix. Here, we use the property
Since matrix is skew-symmetric, we have . Replacing this result into equation (14), we have
which gives the proof of the result.
For a linear Hamiltonian dynamical system, the particle satisfies the law of conservation of energy. Similarly, the generalized energy of the dynamical system (5) conserves constant if we define its generalized energy as
where are entries of matrix .
The generalized energy of the dynamical system (5) conserves constant.
Proof. From the definition of the generalized energy (15), we have
Noticing the trace property and from equation (17), we have
Assume that the generalized energy of the dynamical system (5) conserves constant, then matrix is skew-symmetric.
where . Let vectors and vector in equation (19), we obtain
Namely matrix is skew-symmetric.
Proof. We denote , then from equation (5), we have
which gives the proof of the result.
3 Pseudo-Symplectic Runge-Kutta Methods
Since it does not exist a general linear multiple method to satisfy the symplectic property for a Hamiltonian dynamical system (see Tang1994 ), we consider Runge-Kutta methods with the symplecticity for linear matrix differential equation (5). An s-stage Runge-Kutta method for equation (5) has the following form (see Butcher2003 ) :
The symplectic condition of a Runge-Kutta method for the even dimensional Hamiltonian system is
where is a diagonal matrix and are the coefficients of the Runge-Kutta method (20)-(21) (see Sanz1988 , or Theorem 4.3, p. 192 in HLW2006 , or Theorem 1.4, p. 267 in FQ2010 , or equation (6.14), p. 152 in LR2004 ). For the linear matrix differential equation (5) with the odd dimensional variables, we have the same symplectic condition (22) of the Runge-Kutta method. We state it as the following Theorem 3.1.
Proof. From equation (21), we have
On the other hand, from equation (20), we have
Proof. From the Runge-Kutta method (21), we have
According to equation (20), we obtain
which gives the proof of the result of equation (28) and also gives .
When equals of a Runge-Kutta method (20)-(21) and its coefficients are listed by Table 2, the method is also called the implicit midpoint method with order 2. It is not difficult to verify that the implicit midpoint method satisfies the symplectic condition (22). Therefore, it is a symplectic geometric method.
If we apply the implicit midpoint method to the linear matrix differential equation (5), we have
Here, the Cayley transform
is commutative. Namely equals and the adjoint operator is defined by
Therefore, from equation (32), we obtain
Namely the numerical solutions of the implicit midpoint method preserve the orthogonal invariant.
The Cayley transform is also looked as a composition of the explicit Euler transform
and the implicit Euler transform
That is to say
The adjoint operator of is defined by . If the adjoint operator equals , it is called symmetric.
According to the definition of the symmetric operator, it is not difficult to see that the Cayley transform (33) is symmetric.
4 Numerical Experiments
In order to illustrate the structure-preserving property of the symplectic method for the differential equation (5), we compare the numerical results of the symplectic implicit midpoint method listed by Table 2 with the numerical results of the non-symplectic explicit second order Runge-Kutta method listed by Table 3 Butcher2003 .
When we apply the explicit second order Runge-Kutta method to the linear matrix differential equation (5), we obtain the following iteration formula
It is not difficult to verify that the coefficients of the explicit Runge-Kutta method can not satisfy the symplectic condition (22). This means that its numerical solutions of the explicit Runge-Kutta method can not preserve geometric structure (23) and can not comply with the conservation of energy of the dynamical system (5).
The test problem is given as
The integrated interval is and the fixed time-stepping length is 0.1.
The numerical results of the test problem are presented by Figure 1. The horizontal axis is on time and the vertical axis represents the error of the discrete energy. From Figure 1, we find that the generalized energy of the symplectic implicit midpoint method (32) fluctuates tiny, and the generalized energy of the non-symplectic explicit Runge-Kutta method grows with time. It means that the numerical results conform to the theoretical results of the previous sections.
We mainly extend the applicable fields of symplectic geometric algorithms from the even dimensional Hamiltonian system to the odd dimensional dynamical system, and discuss the geometric and algebraic properties of symplectic Runge-Kutta methods for the linear matrix differential equation, such as the symplecticity and the orthogonality. It is worth noting that the implicit midpoint rule preserves the Lie group structure of orthogonal matrices (see for example p. 118 in HLW2006 ) and this is the interested research topic. Another interesting issue is how to preserve the invariant of when we use the approximate technique in LW2011 if the initial matrix is not orthogonal. We would like to consider those issues in our future work.
Financial and Ethical Disclosures
Funding: This study was funded by by Grant 61876199 from National Natural Science Foundation of China, Grant YBWL2011085 from Huawei Technologies Co., Ltd., and Grant YJCB2011003HI from the Innovation Research Program of Huawei Technologies Co., Ltd..
Conflict of Interest: The authors declare that they have no conflict of interest.
J. C. Butcher,
Numerical Methods for Ordinary Differential Equations, Wiley, 2003.
- (2) R. P. K. Chan, H. Y. Liu and G. Sun, Efficient symplectic Runge¨CKutta methods, Applied Mathematics and Computation, 172 (2006), pp. 908-924.
- (3) P. Corke, Robotics, Vision and Control: Fundamental Algorithms in MATLAB, Springer, Simplified Chinese language edition by Publishing House of Electronics Industry, 2016.
- (4) J. M. Falquez, M. Kasper, G. Sibley, Inertial aided dense & semi-dense methods for robust direct visual odometry, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3601-3607.
- (5) K. Feng and M.-Z. Qin, Symplectic Geometric Algorithms for Hamiltonian Systems, Zhejiang Science and Technology Publishing House and Springer, 2010.
- (6) K. Feng, Z.-J. Shang, Volume-preserving algorithms for source-free dynamical systems, Numerische Mathematik, 71 (1995), pp. 451-463.
- (7) S. Q. Gan, Z. J. Shang and G. Sun, A class of symplectic partitioned Runge-Kutta methods, Applied Mathematics Letters 26 (2013), pp. 968-973.
- (8) E. Hairer, C. Lubich and G. Wanner, Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations (second edition), Springer, 2006.
- (9) D. J. Higham, Time-stepping and preserving orthogonality, BIT 37 (1997), pp. 24-36.
- (10) J. L. Hong, H. Y. Liu and G. Sun, The multi-symplecticity of partitioned Runge-Kutta methods for Hamiltonian PDEs, Mathematics of Computation, 75 (2005), pp. 167-181.
- (11) B. Leimkuhler and S. Reich, Simulating Hamiltonian Dynamics, Cambridge University Press, 2004.
- (12) M. Y. Li and A. I. Mourikis, High-precision, consistent EKF-based visual-inertial odometry, International Journal of Robotics Research, 32 (2013), pp. 690-711.
- (13) X.-L. Luo and Z.-J. Wu, Least-squares approximations in geometric buildup for solving distance geometry problems, Journal of Optimization Theory and Applications, 149 (2011), pp. 580-598.
- (14) MATLAB R2008a, http://www.mathworks.com.
- (15) B. B. MaTBeeB and B. PacOOB, Design Principles of Strapdown Inertial Navigation Systems(Chinese translation from Russian), National Defense Industry Press, Beijing, 2017.
T. Qin, P. L. Li and H. J. Shen,
A robust and versatile monocular visual-inertial state estimator, IEEE Transactions on Robotics, 34 (2018), pp. 1004-1020.
- (17) J. M. Sanz-Serna, Runge-Kutta schemes for Hamiltonian systems, 28 (1988), pp. 877-883.
- (18) G. Sun, Construction of high order symplectic Runge-Kutta methods, Journal of Computational Mathematics, 11 (1993), pp. 250-260.
- (19) G. Sun, A simple way constructing symplectic Runge-Kutta methods, Journal of Computational Mathematics, 18 (2000), pp. 61-68.
- (20) G. Sun, S. Q. Gan, H. Y. Liu and Z. J. Sang, Symmetric-adjoint and symplectic-adjoint methods and their applications, arXiv:1808.055548v1 [math.NA], August 16, 2018.
- (21) K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. k. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, Robust stereo visual inertial odometry for fast autonomous flight, IEEE Robotics and Automation Letters, Accepted, arXiv: 1712.00036 [cs.RO], 2017.
- (22) Y. F. Tang, Formal energy of a symplectic scheme for Hamiltonian systems and its applications, Computers & Mathematics with Applications 27 (1994), pp. 31-39.
- (23) T. Tao, Nonlinear Dispersive Equations: Local and Global Analysis, the American Mathematical Society, 2006.
- (24) D. L. Wang and A. G. Xiao, Parametric symplectic partitioned Runge¨CKutta methods with energy-preserving properties for Hamiltonian systems, Computer Physics Communications, 184 (2013), pp. 303-310.
- (25) A. G. Xiao and Y. F. Tang, Order properties of symplectic Runge-Kutta-Nystrom methods, Computers and Mathematics with Applications, 47 (2004), pp. 569-582.