Symplectic Geometric Methods for Matrix Differential Equations Arising from Inertial Navigation Problems

# Symplectic Geometric Methods for Matrix Differential Equations Arising from Inertial Navigation Problems

## Abstract

This article explores some geometric and algebraic properties of the dynamical system which is represented by matrix differential equations arising from inertial navigation problems, such as the symplecticity and the orthogonality. Furthermore, it extends the applicable fields of symplectic geometric algorithms from the even dimensional Hamiltonian system to the odd dimensional dynamical system. Finally, some numerical experiments are presented and illustrate the theoretical results of this paper.

###### keywords:
symplectic geometric methods, matrix differential equation, inertial navigation, simultaneous localization and mapping, robotic vision
AMS subject classifications. 65H17, 65J15, 65K05, 65L05
1

## 1 Introduction

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 vector and 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

 v=ω×x=⎡⎢⎣0−ω3ω2ω30−ω1−ω2ω10⎤⎥⎦⎡⎢⎣x1x2x3⎤⎥⎦, (1)

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

 y=R(t)x. (2)

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

 (ω×e1,ω×e2,ω×e3)=⎡⎢⎣0−ω3ω2ω30−ω1−ω2ω10⎤⎥⎦=W. (3)

Thus, we obtain , which is the rotational velocity expressed in the stationary frame, by the back transformation (2):

 RT˙R=W,

which gives

 ˙R=RW,R(t0)TR(t0)=I, (4)

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

 ˙Q=SQ,Q(t0)TQ(t0)=I, (5)

where is a skew-symmetric matrix. We denote the solution of linear differential equation (5) as

 Φt(Q(t0))=Q(t;Q(t0)), (6)

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

 df(X)=limΔt→0f(Z+ΔtX)−f(Z)Δt=N∑i=1N∑j=1∂f∂zijxij, (7)

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 ()):

 (df∧dg)(X,Y)=df(X)dg(Y)−df(Y)dg(X), (8)

where . Thus, we give the definition of the wedge product of two matrix functions and as follows (see p. 30, Tao2006 ()):

 dP∧dQ==M∑i=1M∑j=1(dpij∧dqij), (9)

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.

###### Property 2.1

Let be -matrices of differential one-forms on , then the following properties hold.

• Skew-symmetry

 dP∧dQ =−dQ∧dP. (10)
• Bilinearity: for any ,

 dP∧(αdQ+βdR) =αdP∧dQ+βdP∧dR. (11)
• Rule of matrix multiplication

for any matrix .

Now we give the pseudo-symplectic property of equation (5).

###### Property 2.2

Assume that is the solution of equation (5) with an initial orthogonal matrix , then it satisfies the following geometric property

 dQ∧SdQ=const, (13)

where the wedge product is defined by equation (9) and is a constant number.

Proof.  Actually, if is the solution of equation (5), from properties (10)-(12), we have

 ddt(dQ∧SdQ) =d˙Q∧SdQ+dQ∧Sd˙Q=SdQ∧SdQ+STdQ∧d˙Q =−SdQ∧d˙Q=−SdQ∧SdQ=0,

which proves the result of equation (13).

###### Remark 2.1

If the skew-symmetric matrix equals and is the vector function, equation (13) is the condition of symplecticity (see p. 65 in LR2004 ()).

###### Theorem 2.1

(Kang Feng & Zai-Jiu Shang FS1995 ()). Assume that is the solution of the matrix differential equation (5) and matrix is skew-symmetric. Then, the determinant is an invariant.

Proof.  We denote , then it only needs to prove along the solution curve of the matrix differential equation (5). Actually, from equation (5), we have

 g(t +Δt)=det(Q(t+Δt))=det((Q(t)+˙Q(t)Δt+O((Δt)2)) =det((Q(t)+SQ(t)Δt+O((Δt)2))=det(I+SΔt+O((Δt)2))det(Q(t)) =M∏i=1((1+λi(S)Δt+O((Δt)2))det(Q(t)) =(1+trace(S)Δt+O((Δt)2))g(t), (14)

where represents the eigenvalue of matrix . Here, we use the property

 trace(S)=M∑i=1λi(S).

Since matrix is skew-symmetric, we have . Replacing this result into equation (14), we have

 dg(t)dt=limΔt→0g(t+Δt)−g(t)Δt=0,

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

 E(t)=trace(Q(t)TQ(t))=M∑i=1M∑j=1q2ij(t), (15)

where are entries of matrix .

###### Property 2.3

The generalized energy of the dynamical system (5) conserves constant.

Proof.  From the definition of the generalized energy (15), we have

 dE(t)dt=ddttrace(Q(t)TQ(t))=2trace(Q(t)T˙Q(t)). (16)

Since satisfies the dynamical equation (5), replacing with in equation (16), we obtain

 dE(t)dt=2trace(Q(t)TSQ(t)). (17)

Noticing the trace property and from equation (17), we have

 dE(t)dt =2trace(Q(t)TSQ(t))=2trace(Q(t)TSTQ(t)) =−2trace(Q(t)TSQ(t))=0, (18)

where we use the assumption in the above third equality of equation (18). Thus, we prove that the generalized energy of the dynamical system (5) conserves constant.

Another interesting property is about the inverse proposition of property 2.3. We state this property as the following Property 2.4.

###### Property 2.4

Assume that the generalized energy of the dynamical system (5) conserves constant, then matrix is skew-symmetric.

Proof.  Since the generalized energy along the solution of equation (5) conserves constant, from equations (16)-(17) and equation (5), we have

 dE(t)dt =2trace(Q(t)T˙Q(t))=2trace(Q(t)TSQ(t)) =2n∑i=1(q(t)TiSq(t)i)=0, (19)

where . Let vectors and vector in equation (19), we obtain

 sij+sji=0.

Namely matrix is skew-symmetric.

###### Remark 2.2

For the linear matrix differential equation (5), there is a stronger property than Property 2.3. Namely is an invariant (see Theorem 1.6, pp. 99 in HLW2006 ()).

Proof.  We denote , then from equation (5), we have

 dF(t)dt =˙Q(t)TQ(t)+Q(t)T˙Q(t)=Q(t)TSTQ(t)+Q(t)TSQ(t) =−Q(t)TSQ(t)T+Q(t)TSQ(t)=0,

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 ()) :

 Yi=Qk+Δts∑j=1aijSYj,1≤i≤s, (20) Qk+1=Qk+Δts∑i=1biSYi, (21)

where is a time-stepping length and are constants. For a Runge-Kutta method, we can write a condensed representation , which is so-called Butcher-array as Table 1 (see Butcher2003 ()).

The symplectic condition of a Runge-Kutta method for the even dimensional Hamiltonian system is

 M=BA+ATB−bbT=0, (22)

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.

###### Theorem 3.1

Assume that is the solution of equations (20)-(21), when the coefficients of a Runge-Kutta method satisfy the symplectic condition (22), then we have

 dQk+1∧SdQk+1=dQk∧SdQk. (23)

Proof.  From equation (21), we have

 dQk+1∧SdQk+1 =(dQk+Δts∑i=1biSdYi)∧S(dQk+Δts∑i=1biSdYi) =dQk∧SdQk+Δts∑i=1bi(dQk∧S2dYi+SdYi∧SdQk) +(Δt)2s∑i=1s∑j=1(bibjSdYi∧S2dYj). (24)

On the other hand, from equation (20), we have

 SdYi∧SdQk =SdYi∧Sd(Yi−Δts∑j=1aijSYj) =SdYi∧SdYi−Δts∑j=1aij(SdYi∧S2dYj) =−Δts∑j=1aij(SdYi∧S2dYj), (25)

and

 dQk∧S2dYi =(dYi−Δts∑j=1aijSdYj)∧S2dYi =STdYi∧SdYi−Δts∑j=1aij(SdYj∧S2dYi) =−SdYi∧SdYi−Δts∑j=1aij(SdYj∧S2dYi) =−Δts∑j=1aij(SdYj∧S2dYi). (26)

Replacing the results of equations (25)-(26) into equation (24), we obtain

 dQk+1 ∧SdQk+1=dQk∧SdQk +(Δt)2s∑i=1s∑j=1(bibj−biaij−bjaji)(SdYi∧S2dYj). (27)

Thus, from equation (27), we know that the result of equation (23) is true if the coefficients of a Runge-Kutta method satisfy equation (22).

###### Theorem 3.2

Assume that the coefficients of a Runge-Kutta method satisfy the symplectic condition (22) and apply this Runge-Kutta method for the linear matrix differential equation (5) to obtain its numerical solution , then we have

 QTk+1Qk+1=QTkQk=Q(t0)TQ(t0)=I, (28)

which also gives the conservation of the discrete generalized energy .

Proof.  From the Runge-Kutta method (21), we have

 QTk+1Qk+1=QTkQk+Δts∑i=1bi(QTkSYi+YTiSTQk)+(Δt)2s∑i=1s∑j=1bibjYTiSTSYj. (29)

According to equation (20), we obtain

 QTkSYi=YTiSYi−Δts∑j=1aijYTjSTSYi, (30)

and

 YTiSTQk=YTiSTYi−Δts∑j=1aijYTiSTSYj. (31)

Inserting the results of equations (30)-(30) into equation (29), and using the symplectic condition (22), we have

 QTk+1Qk+1 =QTkQk+Δts∑i=1bi(QTkSYi+YTiSTQk)+(Δt)2s∑i=1s∑j=1bibjYTiSTSYj =QTkQk+(Δt)2s∑i=1s∑j=1(biaij+bjaji−bibj)YTiS2Yj=QTkQk,

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

 Qk+1=(I−12ΔtS)−1(I+12ΔtS)Qk. (32)

Here, the Cayley transform

 ΩΔt(S)=(I−12ΔtS)−1(I+12ΔtS) (33)

is commutative. Namely equals and the adjoint operator is defined by

 Ω∗Δt(S)=Ω−1−Δt(S)=(I+12ΔtS)(I−12ΔtS)−1. (34)

When matrix is skew-symmetric, from equations (33)-(34), we have

 ΩΔt(S)TΩΔt(S)=I.

Therefore, from equation (32), we obtain

 QTk+1Qk+1=QTkΩΔt(S)TΩΔt(S)Qk=I.

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

 ΦΔt(S)=(I+ΔtS)

and the implicit Euler transform

 Φ∗Δt(S)=(I−ΔtS)−1.

That is to say

 ΩΔt(S)=Φ∗12Δt(S)Φ12Δt(S).
###### Definition 3.1

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

 Qk+1=(I+ΔtS+12Δt2S2)Qk. (35)

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

 S=⎡⎢⎣02−0.1−2000.100⎤⎥⎦. (36)

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.

## 5 Conclusion

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.

### Footnotes

1. journal: Journal of XXXXX

### References

1. 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.
16. 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.
You are adding the first comment!
How to quickly get a good reply:
• Give credit where it’s due by listing out the positive aspects of a paper before getting into which changes should be made.
• Be specific in your critique, and provide supporting evidence with appropriate references to substantiate general statements.
• Your comment should inspire ideas to flow and help the author improves the paper.

The better we are at sharing our knowledge with each other, the faster we move forward.
The feedback must be of minimum 40 characters and the title a minimum of 5 characters