Jin Wu

Member, IEEE Miaomiao Wang

Hassen Fourati
email: hassen.fourati@gipsa-lab.grenoble-inp.fr

Hui Li

Yilong Zhu

Member, IEEE Chengxi Zhang

Member, IEEE Yi Jiang

Xiangcheng Hu

Senior Member, IEEE Ming Liu

) J Wu

Generalized n-Dimensional Rigid Registration: Theory and Applications

Keywords: Point-cloud Registration, Robotic Perception, Navigation, Rigid Transformation, Covariance Analysis

published or not. The documents may come   L'archive ouverte pluridisciplinaire

I. INTRODUCTION

A. Background and Related Works

Point-cloud registration has been extensively developed in the past few decades and widely employed in various fields including robotic perception, automated reconstruction, computer aided design (CAD), etc. [START_REF] Liu | Robotic Online Path Planning on Point Cloud[END_REF]- [START_REF] Xie | Variance-Minimization Iterative Matching Method for Free-Form Surfaces -Part II : Experiment and Analysis[END_REF]. The points can either be measured by a 2-D/3-D laser scanner, or by a time-offlight (TOF) sensor, or even by scene reconstruction from monocular/binocular cameras [START_REF] Paloj | Integrated Time-of-Flight Laser Radar[END_REF], [START_REF] Liu | Topological Mapping and Scene Recognition with Lightweight Color Descriptors for An Omnidirectional Camera[END_REF]. The basic purpose of the point-cloud registration is to find out the rigid/affine/nonrigid transformations between two measured point sets, while most registration problems can be considered as locally rigid ones as much as possible [START_REF] Bai | Nonrigid Point Set Registration by Preserving Local Connectivity[END_REF], [START_REF] Li | Robust Point Set Registration Using Signature Quadratic Form Distance[END_REF]. By virtue of this aim, many engineering processes require the point-cloud registration, including spacecraft attitude determination, autonomous navigation, simultaneous localization and mapping (SLAM), etc. [START_REF] Tahri | Efficient Iterative Pose Estimation Using an Invariant to Rotations[END_REF]- [START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF]. Thus, the principle of point-cloud registration is to seek the most appropriate correspondences along with an optimal transformation to match the two point sets. Note that the point numbers of the two sets do not have to be consistent, so such problem is usually non-convex during matching. For rigid point-cloud registration, the best correspondences and the optimal rigid transformation are usually unified with the iterative closest point (ICP, [START_REF] Besl | A Method for Registration of 3-D Shapes[END_REF]). In ICP, the correspondences can be figured out iteratively by means of brute-force searching or aided by a kD tree, with a given transformation guess. The rigid transformation is often obtained via closed-form solutions, e.g., singular value decomposition (SVD, [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF], [START_REF] Umeyama | Least-Squares Estimation of Transformation Parameters Between Two Point Patterns[END_REF]), eigendecomposition (EIG, [START_REF] Horn | Closed-form Solution of Absolute Orientation Using Unit Quaternions[END_REF]), dual quaternion method [START_REF] Walker | Estimating 3-D Location Parameters Using Dual Number Quaternions[END_REF], etc. [START_REF] Wu | Fast Symbolic 3-D Registration Solution[END_REF]. ICP is practical and efficient but suffers from local minima during iterations. Therefore, some efforts have been paid to find the global optimum of such problem, e.g., the Go-ICP [START_REF] Yang | Go-ICP: A Globally Optimal Solution to 3-D ICP Point-Set Registration[END_REF]. Mainstream ICP variants mainly deal with the 3-D registration case while the high-dimensional one is actually needed for some other cases, such as the localization of sensor networks [START_REF] Sanyal | A Scalable ADMM Algorithm for Rigid Registration[END_REF], [START_REF] Sanyal | On a Registration-Based Approach to Sensor Network Localization[END_REF].

The main work presented in this paper is to show the closed-form solution and its covariance analysis of such highdimensional rigid registration problem. To describe the shape of rigid transformations, special orthogonal groups and special Euclidean groups are usually invoked [START_REF] Hashim | Nonlinear Stochastic Attitude Filters on the Special Orthogonal Group 3: Ito and Stratonovich[END_REF], [START_REF] Hashim | Nonlinear Pose Filters on the Special Euclidean Group SE(3) with Guaranteed Transient and Steady-state Performance[END_REF]. Studying the control problems based on such manifolds has become popular in recent years, involving some results on feedback control laws, optimization hull, motion planning, etc. [START_REF] Markdahl | Exact Solutions to A Class of Feedback Systems on SO(n)[END_REF]- [START_REF] Biggs | Motion Planning on A Class of 6-D Lie Groups via A Covering Map[END_REF]. The specific problem of n-dimensional rigid registration can be solved via the SVD or linear matrix inequality (LMI) [START_REF] Du | Affine Iterative Closest Point Algorithm for Point Set Registration[END_REF], [START_REF] Ruiter | On the Solution of Wahba's Problem on SO(n)[END_REF]. However, as both these methods suffer from the existence of high nonlinearity, a covariance analysis guaranteeing reliable quality control may be not feasible at the current stage.

The engineering background for the presented study is that, apart from those nonlinear observers on the 3-D special orthogonal groups [START_REF] Mahony | Nonlinear Complementary Filters on The Special Orthogonal Group[END_REF], [START_REF] Berkane | On the Design of Attitude Complementary Filters on SO(3)[END_REF], higher dimensional registration techniques have been ultilized by the authors in [START_REF] Wu | Hand-eye Calibration: 4-D Procrustes Analysis Approach[END_REF] to solve the robotic hand-eye calibration between robotic gripper and attached camera, of the type AX = XB with A, B being known and X being unknown. It is revealed that a mapping from 3-D special Euclidean group to the 4-D special orthogonal group will be of convenience in solving such problem. In the 4-D case, the authors propose the unit octonion method for point-cloud registration but this is not extendable regarding registration with arbitrary dimensions. As hand-eye calibration has received extensive research during the past several decades [START_REF] Tsai | A New Technique for Fully Autonomous and Efficient 3-D Robotics Hand/Eye Calibration[END_REF]- [START_REF] Condurache | Orthogonal Dual Tensor Method for Solving the AX = XB Sensor Calibration Problem[END_REF], the proposed n-dimensional case may benefit future related works. It is also noticed that, for the 3-D case, Barczyk et al. have derived the closed form of the ICP matching covariance and the results have been later fused with inertial measurement using an invariant Kalman filter [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF]. As mentioned above, the 3-D and 4-D cases are all specific ones that can not give mandatory information for the extension to n-dimensional ones. Therefore the main challenge confronted is to find an efficient universal parameterization approach for the registration on n-dimensional Euclidean space.

B. Contributions

Following above problems in n-dimensional registration, this paper proposes a new formulation as linear solution. Major contributions are:

• Based on Caylay transformation, linear results regarding the registration problem have been derived. The developed method provides a new perspective other than existing ones like SVD. • Computational burden has been significantly decreased by simplifying related computation steps via specific matrix manipulations. With this technique the online efficiency of the algorithm has been improved. • The uncertainty descriptions of the derived solution are also derived which gives the quantization of the quality of registration with given noisy point clouds. Following these contributions, through simulation and experimental results, we also show the advantages of the proposed method in algorithmic implementation and efficiency, compared with representatives like LMI, SVD and other applications.

C. Outline

This paper is structured as follows: Section II presents the problem formulation and introduces our proposed linear solution and covariance analysis. Section III consists of experiments, results and comparisons while concluding remarks are drawn in Section IV.

D. Notations

The n-dimensional Euclidean vector space is described with R n . We use R n×m to denote the real space containing all matrices with row dimension of n and column dimension of m. The identity matrix has the notation of I and owns a certain size according to the context. X ⊤ , X -1 mean the transpose and inverse of a given matrix X respectively in which the inverse exists when X is square and nonsingular. We use tr to represent the trace of a square matrix. The adj denotes the adjoint matrix. ∥•∥ stands for the l 2 norm in the Euclidean space such that ∥x∥ =

√

x ⊤ x for any given column vector x. rank(X) depicts the row rank information of X. For the three dimensional Lie algebra, the special orthogonal group SO(3) contains all the orthonormal rotation matrices in R 3×3 . It is extended to n-dimensional Euclidean space with the SO(n) whose identity is expressed with

X ∈ SO(n) ⇒ XX ⊤ = X ⊤ X = I, det(X) = 1.
The so(3) contains all skew-symmetric matrices x × from any 3-dimensional vector x = (x 1 , x 2 , x 3 ) ⊤ such that the cross product between any two 3-dimensional vectors x, y is equivalent to x × y = x × y = -y × x. The generalization of so(3) from 3-D space to the n-D space is so(n). Note that any element on the group so(n) is a skew-symmetric matrix and can be exponentially mapped to a unique rotation matrix on

SO(n). With a n(n-1) 2 -D vector x = x 1 , x 2 , . . . , x n(n-1) 2 ⊤
, the associated skew-symmetric matrix is defined in (1) so that x × ∈ so(n).

x × =                   0 -x n(n-1) 2
x n(n-1)

2 -1 • • • (-1) n-2 x 2n-3 (-1) n-1 x n-1 * 0 -x n(n-1) 2 -2 • • • (-1) n-3 x 2n-4 (-1) n-2 x n-2 * * . . . • • • . . . . . .
(1) The inverse map, i.e., the wedge operation ∧ from the n × n skew-symmetric matrix to the n(n-1)

2

-D vector is denoted as

x ∧ × = x.

II. HIGH-DIMENSIONAL REGISTRATION: SOLUTION AND COVARIANCES A. Problem Formulation

The generalized rigid registration problem can be characterized with the following optimization [START_REF] Besl | A Method for Registration of 3-D Shapes[END_REF],

arg min R∈SO(n),t∈R 3 L = N i=1 w i ∥b i -Rr i -t∥ 2 (3)
in which R is the n-dimensional rotation matrix; t represents the Euclidean translation vector in the R n . The rotation and translation, together, namely the homogeneous transformation R t 0 1 ∈ SE(n) in the special Euclidean group SE(n), relates N vector pairs {b i , r i |i = 1, 2, . . . , N } in the body frame b and reference frame r together. Here the relationship between vector pairs is expressed with the normalized weight

w i , such that N i=1 w i = 1.
The weights actually denote the uncertainty characteristics of the data to be aligned. However, in practice, when there are large amount of points, e.g. for the Lidar mapping, there is no criteria to determine the weights.

In that case, the weights are computed equally as w i = 1/N . The problem (3) will always be convex by introducing the unit quaternion for representation of R in SO(3) [START_REF] Wu | Generalized Linear Quaternion Complementary Filter for Attitude Estimation from Multi-Sensor Observations : An Optimization Approach[END_REF]. The uniqueness of the problem can be found in [START_REF] Umeyama | Least-Squares Estimation of Transformation Parameters Between Two Point Patterns[END_REF]. The problem is a least-square one and can be solved with many techniques [START_REF] Mohammadi | A Divergence-Free Generalized Moving Least Squares Approximation with its Application[END_REF]. When the dimension increases, the quaternion will be no longer feasible to give adequate description of rotations.

Let us think about the general rotation representation and factorization that is independent of the dimension n. For any orthonormal rotation matrix with dimension of three, one has Rξ = ξ, where ξ = (ξ 1 , ξ 2 , ξ 3 ) ⊤ is called the eigenaxis of R.

While for the rotation in SO(3), the Rodrigues formula can be accordingly derived as R = cos θI +(1-cos θ)ξξ ⊤ +sin θξ × , where θ is the intermediate rotation angle about the eigenaxis ξ.

The high-dimensional extension of Rodrigues formula is in a sophisticated form since the cross-product of vectors are only proven to exist in three and seven dimensional Euclidean spaces [START_REF] Massey | Cross Products of Vectors in Higher Dimensional Euclidean Spaces[END_REF]. Moreover, the Rodrigues formula is nonlinear and can hardly offer convenience for rotation computation. However, the Cayley transformation, i.e.,

R = (I + G) -1 (I -G) (4)
with G denoting an n-dimensional skew-symmetric matrix, always holds for the rotation factorization. Such technique has been invoked for solving the Wahba's problem that is a special case of (3) with n = 3, t = 0 and ∥b i ∥ = ∥r i ∥ = 1 forming a spacecraft attitude estimator called OLAE [START_REF] Mortari | Optimal Linear Attitude Estimator[END_REF]. With different data dimensions n > 3, the problem significantly varies. The matrices I + G and I -G are often invertible, but will suffer from singularities when all Euler angles approach ±π, which is a special case. However, in engineering, there is almost no such a coincident case for n-dimensional registration, because of the noise of the input data. Therefore, we assume here that I + G and I -G are strictly invertible. In the following parts, we are going to present the proposed linear solution and associated covariance analysis.

B. Proposed Linear Solution

Let us define the centers of the mass of the point sets as:

b = N i=1 w i b i , r = N i=1 w i r i (5)
with b i , r i ∈ R n . For large numbers of points, without loss of generality, the covariance of each point can be unified with Σ = Σ bi = Σ ri . Then the rotation-only problem is converted to the following optimization arg min

R∈SO(n) L = N i=1 w i b i -b -R(r i -r) 2 (6)
With the virtue of (4) and defining bi = b i -b, ri = r i -r and the error vector e i = bi -Rr i , one always expect the error to be zero, such that

e i = bi -(I + G) -1 (I -G)r i = 0. (7)
The right item can be further expressed as

bi = (I + G) -1 (I -G) ri ⇒ (I + G) bi = (I -G) ri ⇒ G bi + ri = ri -bi . (8
) Note that in [START_REF] Mortari | Optimal Linear Attitude Estimator[END_REF], the above equation can be further derived to bi + ri

× g = bi -ri (9)
where G is replaced by G = g × ∈ so(3). The equation can be obtained by symbolic manipulation using MATLAB, Maple or Mathematica. That is to say the minimization ( 6) is transformed into arg min

G ⊤ =-G L = N i=1 w i ∥Gx i -d i ∥ 2 (10)
where

x i = bi + ri d i = ri -bi . ( 11
)
For the n-dimensional G, we need to determine the n(n-1)/2 items inside G such that

G(g) = g ⊗ =          0 g 1 g 2 • • • g n-1 -g 1 0 g n • • • g 2n-3 -g 2 g n . . . • • • . . . . . . . . . . . . 0 g n(n-1) 2 -g n-1 -g 2n-3 • • • -g n(n-1) 2 0          (12)
with g = g 1 , g 2 , . . . , g n(n-1)

2

⊤

and G being a linear function of g. It can be noticed that although the linear mapping g ⊗ is also on so(n), it does not require the specific arrangements of elements and signs that are needed inside the mapping from G to g × in [START_REF] Liu | Robotic Online Path Planning on Point Cloud[END_REF]. Therefore, g ⊗ is more flexible than g × in representing G (see skewdec command in MATLAB software for some details). Then for arbitrary Gx i in [START_REF] Wu | Fast Linear Quaternion Attitude Estimator Using Vector Observations[END_REF], we can always find a corresponding matrix P n (x i ) satisfying G(g)x i = P n (x i )g. This can be achieved via many concurrent mathematical tools. For instance, in the MATLAB, we

P 4 (x i ) =     x i,2 x i,3 x i,4 0 0 0 -x i,1 0 0 x i,3 x i,4 0 0 -x i,1 0 -x i,2 0 x i,4 0 0 -x i,1 0 -x i,2 -x i,3     P 5 (x i ) =       x i,2 x i,3 x i,4 x i,5 0 0 0 0 0 0 -x i,1 0 0 0 x i,3 x i,4 x i,5 0 0 0 0 -x i,1 0 0 -x i,2 0 0 x i,4 x i,5 0 0 0 -x i,1 0 0 -x i,2 0 -x i,3 0 x i,5 0 0 0 -x i,1 0 0 -x i,2 0 -x i,3 -x i,4       (2)
P ⊤ 4 (x i )P 4 (x i ) =               x 2 i,1 + x 2 i,2 xi,2xi,3 xi,2xi,4 xi,2xi,3 x 2 i,1 + x 2 i,3 xi,3xi,4 xi,2xi,4 xi,3xi,4 x 2 i,1 + x 2 i,4 S 1 . . . . . . x 2 i,2 + x 2 i,3 xi,3xi,4 xi,3xi,4 x 2 i,2 + x 2 i,4 S 2 . . . . . . x 2 i,3 + x 2 i,4 S 3               (14)
can use equationsToMatrix function to obtain instant expression of P i . The optimization ( 10) is then solved by

∂L ∂g = 0 ⇒ g ⊤ N i=1 w i P ⊤ (x i )P (x i ) - N i=1 w i d ⊤ i P (x i ) = 0 ⇒ N i=1 w i P ⊤ (x i )P (x i )g = N i=1 w i P ⊤ (x i )d i . (13)
It is obvious that the solution is linear and it follows:

g = H -1 v (15)
with

H = N i=1 w i P ⊤ (x i )P (x i ) v = N i=1 w i P ⊤ (x i )d i . (16)
Then the rotation is reconstructed by

R = (I + g ⊗ ) -1 (I -g ⊗ ) . (17)
As a convention, the translation can be determined with t = b -Rr [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF]. Now that we have already known

P (x i ) ∈ R n× n(n-1)

2

. The time complexity of the matrix inversion varies from O(n 2.373 ) to O(n 3 ) using optimized Coppersmith-Winograd method and the Schoolbook multiplication respectively [START_REF] Bürgisser | Algebraic Complexity Theory[END_REF]. The matrix

H ∈ R n(n-1) 2 × n(n-1) 2
will consume huge load of time to compute its inversion with increasing order n → +∞. Thus the emerging task is to figure out a faster matrix inversion of H by dimension reduction through the following properties.

C. Fast Matrix Inversion

The diagonal zero elements of G make P (x i ) a matrix with row-rank of n -1, which also leads to

rank P ⊤ (x i )P (x i ) = n -1. (18)
With increasing point numbers, we have

rank (H) ⩽ N i=1 rank w i P ⊤ (x i )P (x i ) = N (n -1) . ( 19
)
That is to say only a few pairs of points can hardly obtain accurate registration result of R and t. This requirement is easy to be satisfied as the point numbers are usually far larger than the dimension so as to guarantee the global observability of parameters to be estimated.

Here we define P n (x i ) as the P (x i ) for n-dimensional registration problem. By examples we can see the contents of P n (x i ) with various dimensions in (2). Then we can summarize the generalized form for n-dimensional case, i.e.,

P n (x i ) =        n 2 x ⊤ i n 3 x ⊤ i -xi,1I . . . n n-1 x ⊤ i -xi,2I . . . -xi,nI        (20)
where k j x i denotes the partial column vector consisting of sequential components of x i from index j up to index k. With the aid of explicit form of matrix such as those in (2), one can immediately infer that P ⊤ n (x i )P n (x i ) takes following form

P ⊤ n (x i )P n (x i ) =     S1 . . . S2 . . . . . . Sn-2     (21)
in which S 1 , S 2 , • • • , S n-2 are symmetric positive semidefinite matrices. Taking the fourth-order transformation as example, the matrix manipulation is shown in [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF]. Moreover, for S k we have the following eigenvalues

λ S k = x 2 i,k , x 2 i,k , . . . , x 2 i,k , n j=k x 2 i,j (n-k) eigenvalues. (22)
Then S k is positive semidefinite as all its eigenvalues are nonnegative. Therefore the sum H is also symmetry-preserving and positive semidefinite and takes the summed form of [START_REF] Sanyal | On a Registration-Based Approach to Sensor Network Localization[END_REF].

Invoking the Sherman-Morrison-Woodbury formula

(A + BDC) -1 = A -1 -A -1 B D -1 + CA -1 B -1 CA -1
(23) where A, D and D -1 + CA -1 B are nonsingular matrices, we have

A B C D -1 = A -BD -1 C -1 -A -1 B D -CA -1 B -1 -D -1 C A -BD -1 C -1 D -CA -1 B -1 Notice that D -CA -1 B -1 = D -1 + D -1 C A -BD -1 C -1 BD -1 , (24)
we actually need to pay more attention to solving

A -1 , D -1 and A -BD -1 C -1 . Here A -1 and A -BD -1 C -1
have the same dimension and require the complexity of O[(n -1) 3 ]. Empirically, we found that all S k matrices for k = 1, 2, . . . are strictly invertible when the problem ( 4) is valid, i.e., 1) the measurement number N is greater than than the unknown degree n(n -1)/2 + n; 2) not all the points are coplanar or colinear. Then following the mapping of H to A B C D , we can see that D here is also in the form of H, i.e., the sum of multiple [START_REF] Sanyal | On a Registration-Based Approach to Sensor Network Localization[END_REF]. In this way, H can be iteratively built up by smaller matrix blocks and the final computation comes from these blocks.

In numerical calculation of H -1 , H may always suffer from very tiny or very large determinant values. At such time, the inverse of H will be influenced by the numerical loss. Using the above proposed fast blocked-matrix inversion, we may find out that A -1 , D -1 and A -BD -1 C -1 are required. While seen from ( 21), one can obviously notice that A, D are positive semidefinite. That is to say in engineering tasks, with sufficient point numbers, A, D will be exactly positive definite and this solver is able to obtain much more robust blocked inversions. By taking matrix manipulations including addition and multiplication, H -1 can be accurately recovered. This shows the better robustness of this approach than conventional numerical schemes for computing H -1 .

D. Singularity Avoidance

In [START_REF] Horn | Closed-form Solution of Absolute Orientation Using Unit Quaternions[END_REF], each independent component P ⊤ (x i )P (x i ) is singular due to the rank shown in [START_REF] Wu | Fast Symbolic 3-D Registration Solution[END_REF]. However, with the sum of hetrogeneous matrices P ⊤ (x i )P (x i ), the matrix gradually reaches to full-rank state, which has been described in [START_REF] Yang | Go-ICP: A Globally Optimal Solution to 3-D ICP Point-Set Registration[END_REF]. When some extreme cases occur, H will be singular and the proposed fast inversion in the last subsection will also be trivial. The extreme case mainly covers:

1) Lack of mandatory point numbers [START_REF] Wu | Fast Linear Quaternion Attitude Estimator Using Vector Observations[END_REF].

2) Collinear reference vectors [START_REF] Wu | Recursive Linear Continuous Quaternion Attitude Estimator from Vector Observations[END_REF].

3) Dominant weights [START_REF] Markley | Wahba's Problem with One Dominant Observation[END_REF]. Each of these factors shown above will lead to numerical problem such that the matrix H is rank-deficient. In fact for almost all extreme cases, the determination of rotation would be meaningless as the observability of angles has been significantly distorted. Studying the extreme performance for the proposed method is to prevent the computation from numerical crash, e.g., the existence of Not-a-Number (NaN). For instance, when H is singular, the determinant will be very small which generates very large values of H -1 by

H -1 = adj(H)/ det(H) (25)
Then inserting the large values into (4) results in another huge numerical loss. Under such circumstance, we should replace the original inversion with the pseudo inverse [START_REF] Gebre-Egziabher | MAV Attitude Determination by Vector Matching[END_REF]

g = H + v.
Here H + is the Moore-Penrose generalized inverse of H.

E. Covariance Analysis

There are some assumptions for us to derive the covariances of the obtained results in previous sub-sections:

1) The measured points from one point set contain no correlated covariance between each other. 2) The two point sets for registration have independent noise distribution.

These assumptions are based on the fact that current Lidar sensor outputs huge loads of points, such as Velodyne VLP-16 Lidar can have 300, 000 points per second. Thus one can hardly describe individual correlation inside points. We use ⟨•⟩ to represent the operation of expectation and δ is employed to represent an perturbed infinitesimal induced by input noises from b i and r i . In this sub-section, high-order (second-order or higher) infinitesimals are ignored for their tiny impacts in the error propagation. Then the difference of H is

δH = N i=1 wiP ⊤ (xi + δxi) P (xi + δxi) - N i=1 wiP ⊤ (xi) P (xi) = N i=1 wi P ⊤ (δxi) P (xi) + P ⊤ (xi) P (δxi) + P ⊤ (δxi) P (δxi) ≈ N i=1
wi P ⊤ (δxi) P (xi) + P ⊤ (xi) P (δxi) .

(26) Note that for each sub-item J i , it follows that

J i = w i P ⊤ (δx i ) P (x i ) + P ⊤ (x i ) P (δx i ) . (27)
It can be decomposed linearly in δx i , such that

J i = K 1 (x i ) δx i , K 2 (x i ) δx i , • • • , K n(n-1) 2 (x i ) δx i . (28
) For instance when n = 4, the matrices are

K 1 (x i ) = w i         2x i,1 2x i,2 0 0 0 x i,3 x i,2 0 0 x i,4 0 x i,2 -x i,3 0 -x i,1 0 -x i,4 0 0 -x i,1 0 0 0 0         K 2 (x i ) = w i         0 x i,3 x i,2 0 2x i,1 0 2x i,3 0 0 0 x i,4 x i,3 x i,2 x i,1 0 0 0 0 0 0 -x i,4 0 0 -x i,1         K3 (xi) = wi        0 xi,4 0 xi,2 0 0 xi,4 xi,3 2xi,1 0 0 2xi,4 0 0 0 0 xi,2 xi,1 0 0 xi,3 0 xi,1 0        K4 (xi) = wi        -xi,3 0 -xi,1 0 xi,2 xi,1 0 0 0 0 0 0 0 2xi,2 2xi,3 0 0 0 xi,4 xi,3 0 -xi,4 0 -xi,2        K5 (xi) = wi        -xi,4 0 0 -xi,1 0 0 0 0 xi,2 xi,1 0 0 0 0 xi,4 xi,3 0 2xi,2 0 2xi,4 0 xi,3 xi,2 0        K6 (xi) = wi        0 0 0 0 -xi,4 0 0 -xi,1 xi,3 0 xi,1 0 0 -xi,4 0 -xi,2 0 xi,3 xi,2 0 0 0 2xi,3 2xi,4        .
The noise-perturbed system is given as follows,

(H + δH) (g + δg) = v + δv ⇒ δHg + (H + δH) δg = δv ⇒ δg = H + (δv -δHg) (30)
where

δHg = N i=1 Jig = N i=1 n(n-1) 2 j=1 gjKj (xi) δxi = N i=1    n(n-1) 2 j=1 gjKj (xi)    δxi.
(31) We obtain

Σg = δgδg ⊤ = H + (δv -δHg) (δv -δHg) ⊤ H + = H + δvδv ⊤ + δHgg ⊤ δH -δHgδv ⊤ -δvg ⊤ δH H + = H +                             Σv+ N i=1    n(n-1) 2 j=1 gjKj (xi)    Σx i × N i=1    n(n-1) 2 j=1 gjK ⊤ j (xi)   - N i=1    n(n-1) 2 j=1 gjKj (xi)    δxiδv ⊤ - δxiδv ⊤ N i=1    n(n-1) 2 j=1 gjK ⊤ j (xi)                                H + (32)
in which

Σv = δvδv ⊤ ≈ N i=1 w 2 i P ⊤ (δxi) di + P ⊤ (xi) δdi P ⊤ (δxi) di + P ⊤ (xi) δdi ⊤ = N i=1 w 2 i P ⊤ (di) Σx i P (di) -P ⊤ (xi) Σ x i ,d i P (di) - P ⊤ (di) Σ x i ,d i P (xi) + P ⊤ (xi) Σ d i P (xi) (33)
and

δx i δv ⊤ = δx i N i=1 w i P ⊤ (δx i ) d i + P ⊤ (x i ) δd i ⊤ = N i=1 w i δx i d ⊤ i P (δx i ) + δx i δd ⊤ i P (x i ) = N i=1 w i δx i δd ⊤ i P (x i ) -δx i δx ⊤ i P (d i ) = N i=1 w i [Σ xi,di P (x i ) -Σ xi P (d i )], (34)
provided that

δv = N i=1 w i P ⊤ (δx i ) d i + P ⊤ (x i ) δd i (35)
and

Σ xi,di = δx i δd ⊤ i = δ bi + δr i δr i -δ bi ⊤ = δ bi δr ⊤ i + δr i δr ⊤ i -δ bi δ b⊤ i -δr i δ b⊤ i = δ bi δr ⊤ i + δr i δr ⊤ i -δ bi δ b⊤ i -δr i δ b⊤ i = Σ ri -Σ bi . (36)
In particular, when bi and ri have the same statistical distribution, especially in the cases with huge numbers of points, the covariance of g is

Σg = H + ×         N i=1 wi P ⊤ (di) Σx i P (di) + P ⊤ (xi) Σ d i P (xi) + N i=1    n(n-1) 2 j=1 gjKj (xi)    Σx i N i=1    n(n-1) 2 j=1 gjK ⊤ j (xi)            H + .
(37) While in applications we always need uncertainty description of R. In this way, one may derive

(I + G) R = I -G [I + G (g + δg)] (R + δR) = I -G (g + δg) ⇒ (R + δR) + G (g + δg) (R + δR) = I -G (g + δg) ⇒ δR + GδR + G (δg) R = -G (δg) ⇒ δR = -(I + G) -1 δg ⊗ (R + I) . ( 38
)
Let us write R + I in columns as R + I = (s 1 , s 2 , . . . , s n ).

Then one has

δg ⊗ (R + I) = [P (s 1 ) δg, P (s 2 ) δg, • • • , P (s n ) δg] . (39) Finally the covariance of R is Σ R = δRδR ⊤ = (I + G) -1 δg ⊗ (R + I) (R + I) ⊤ δg ⊗ (I + G) -1 ⊤ = (I + G) -1 n k=1 P (s k ) δgδg ⊤ P ⊤ (s k ) (I + G) -1 ⊤ = (I + G) -1 n k=1 P (s k ) ΣgP ⊤ (s k ) (I + G) -1 ⊤ . (40)
Likewise, the error model of t is δt = δb-δRr -Rδr, which leads to

Σt ≈ Σ b + RΣrR ⊤ + (I + G) -1 δg ⊗ (R + I) r r⊤ (R + I) ⊤ δg ⊗ (I + G) -1 ⊤ ≈ Σ b + RΣrR ⊤ + (I + G) -1 n k=1 P (r k s k ) ΣgP ⊤ (r k s k ) (I + G) -1 ⊤ . (41)

III. EXPERIMENTAL RESULTS

A. Accuracy and Robustness

The algorithmic flow is show in Algorithm 1. For the subsections III.A and III.B, we simulate the following vector pairs

b i = R true r i + t true + ε, i = 1, 2, . . . , N (42)
Algorithm 1 The Generalized Linear n-Dimensional Registration.

Require: Point clouds with N pairs of measurements r i and b i for i = 1, 2, . . . , N .

Step 1:

Compute mean points b = n i=1 a i b i , r = n i=1 a i r i .
Step 2: Compute H matrix and v vector using [START_REF] Sanyal | On a Registration-Based Approach to Sensor Network Localization[END_REF]. If N is large, use results in ( 25) ∼ (30) to simplify the process.

Step 3: Compute required elements of g in ( 20) and then compute the rotation using [START_REF] Hashim | Nonlinear Stochastic Attitude Filters on the Special Orthogonal Group 3: Ito and Stratonovich[END_REF].

Step 4: Reconstruct the translation by t = b -C r.

Step 5: Compute the covariances of R and t by ( 46) and ( 47) respectively.

where ε ∼ N (0, εI) is the zero-mean random perturbation subject to Gaussian distribution and R true , t true are true reference rotation matrix and translation vector. ε here is called the noise density that decides the magnitude of the noise. The accuracy of the n-dimensional registration will be affected by the value of ε, i.e., with larger ε the estimation accuracy will be worse. Six cases with different values of ε are simulated uniformly where the point number N = 100000 to guarantee the avarage statistical performance. The following error criterion is invoked,

ϵ = 1 N N i=1 ∥b i -Rr i -t∥ 2 . ( 43
)
We pick up the SVD [START_REF] Du | Affine Registration of Point Sets Using ICP and ICA[END_REF] and LMI [START_REF] Ruiter | On the Solution of Wahba's Problem on SO(n)[END_REF] approaches for comparison. The SVD uses the following principle with SVD of Z such that

R = U diag [1, 1, . . . , det(U V )] V ⊤ (44)
U ΠV ⊤ = Z Z = 1 N N i=1 b i -b (r i -r) ⊤ ( 45
)
where Π is the diagonal matrix containing all singular values of Z. The LMI is based on the following optimization arg max

R∈SO(n) tr(RZ ⊤ ) (46)
subject to I R ⊤ R I ≥ 0. (47)
All these algorithms are implemented using the C++ programming language of the standard 2014 and compiled with the GNU gcc 7.0 compiler. We use Eigen library to compute the SVD and related matrix manipulations. The GpoSolver is adopted for the solution of LMI [START_REF] Heller | GpoSolver: A MATLAB/C++ Toolbox for Global Polynomial Optimization[END_REF]. Each algorithm is executed for 100 times for mean errors. Simulations on different dimensions are also reflected in the presented tables.

The results are shown in Table I, II, III, IV, V and VI. One can see that with low values of ε, all the algorithms are able to generate accurate registration results. Among them, the SVD owns the best precision and the proposed method ranks the second. With growing ε, the noise also grows, inducing higher errors to the model [START_REF] Bürgisser | Algebraic Complexity Theory[END_REF]. The proposed algorithm now has the same accuracy with that of SVD while the LMI gradually performs badly. The reason is that SVD and the proposed algorithm share the same optimization framework and all belong to analytical solutions. However, LMI converts the rigid-registration loss function into a polynomial optimization one that requires sophisticated nonlinear solver. As SVD has been regarded as a highly reliable algorithm [START_REF] Markley | Attitude Determination Using Vector Observations and the Singular Value Decomposition[END_REF], [START_REF] Forbes | Linear-Matrix-Inequality-Based Solution to Wahba's Problem[END_REF], that is to say the proposed method is stable and reliable in this sense.

B. Computational Efficiency

There is slight impact of the point numbers on the computational efficiency since the complexity of the algorithm is linear with respect to the point numbers, as shown in [START_REF] Umeyama | Least-Squares Estimation of Transformation Parameters Between Two Point Patterns[END_REF]. For high-order matrices, the proposed matrix inversion will gradually be slower than SVD. This is because the proposed one has the complexity of O[ 1 2 n(n -1)] 3 while that of the SVD is O(n 3 ). Choosing the slowest algorithm as a basic reference, the realistic time complexity of the other algorithms should be compentated for specific machines. This leads to the modified time complexity expressions for the proposed method and SVD i.e. O(a i n 3 ) with a i the compensation constant for i-th algorithm. For instance, if we implement the matrix operations with the famous Eigen library using C++ programming language on a MacBook Pro 2017 13" with CPU of i7-4core 3.5GHz, the measured mean values (100 times) are a 2 /a 1 = 1.667 × 10 5 and a 3 /a 1 = 6.25 × 10 6 . Then the plots of time complexity can be shown in Fig. 1. The unaltered matrix inversion using Eigen is very slow in high dimensions and has very large slopes as the dimension n increases. It should be noted that a i is determined by the machine and employed library. For lower dimensions, the computation time would be much lower than the SVD as lowdimensional matrix inversions can usually be obtained with algebraic results instantly. The intersection point between the proposed method and SVD is n = 112. A detailed comparison showing the exact in-run performance of the computation time is presented in Table VII where t denotes the evaluated computation time.

We may see that with increasing dimension n, the computational complexity also increases. At the initial stage, the SVD is quite slower than the proposed method and the LMI is always the slowest since it needs nonlinear optimizations. However, when n becomes larger, the matrix manipulations of the proposed method also turn to be more and more time-consuming. This leads to the deceasing ratio of t SVD /t Proposed . That is to say, for very large n, SVD will be faster and the LMI will be even more complex than the proposed method, as indicated by t LMI /t Proposed (also see Fig. 2). Note that there are very few cases that n is larger than 100, the proposed method is more practical than SVD in most cases since it also owns information of covariances.

C. Application: Covariance-Preserving Interpolation on special Euclidean group SE(n)

The special Euclidean group SE(n) is a simultaneous precise description of the rotation and translation, which has been extensively used in rigid-body state estimation and control [START_REF] Hashim | Nonlinear Pose Filters on the Special Euclidean Group SE(3) with Guaranteed Transient and Steady-state Performance[END_REF], [START_REF] Wang | Hybrid Pose and Velocity-bias Estimation on SE(3) Using Inertial and Landmark Measurements[END_REF], [START_REF] Baldwin | Complementary Filter Design on the Special Euclidean Group SE(3)[END_REF]. Also, in the field of robotics, SE(n) plays important roles in camera egomotion estimation, handeye calibration, motion planning and etc. [START_REF] Biggs | Motion Planning on A Class of 6-D Lie Groups via A Covering Map[END_REF], [START_REF] Wu | Hand-eye Calibration: 4-D Procrustes Analysis Approach[END_REF], [START_REF] Gao | Complete Solution Classification for the Perspective-Three-Point Problem[END_REF]. However, in engineering applications, the measurements on SE(n) are usually restricted by low sampling frequency and asynchronous data transmission. Therefore, interpolated result between two successive measurements on SE(n) will be vital to such systems. The interpolation of SE(n) is sometimes dependent on the interpolation on SO(n). Belta and Kumar studied the interpolation on SE(3) using SVD, where the rotation and translation are simultaneously interpolated for optimal approximation with error of SE(3) geodesics. Because of the internal nonlinearity of SVD, it is very hard for the users to obtain the closed-form information of covariance of the interpolated results. The probabilistic descriptions of interpolation would be of great significance for a later covariance-required application e.g. a Kalman filter on the Lie group [START_REF] Bonnabel | Symmetry-Preserving Observers[END_REF], [START_REF] De Ruiter | Discrete-Time SO(n)-Constrained Kalman Filtering[END_REF]. Therefore our next task is to present a new framework for such purpose.

In [START_REF] Thomas | Approaching Dual Quaternions from Matrix Algebra[END_REF], Thomas gave an approximation from SE(3) to SO(4), which is later improved in [START_REF] Wu | Hand-eye Calibration: 4-D Procrustes Analysis Approach[END_REF]. We have proposed a new generalization such that the following mapping F from SE(n) to SO(n + 1), namely, SE(n) + + [START_REF] Wu | SE(n)++: An Efficient Solution to Multiple Pose Estimation Problems[END_REF],

T = R t 0 1 ∈ SE(n) F ←→ F -1 R T ,SO(n+1) = R εt -εt ⊤ R 1 (48)
holds. Note that here R T ,SO(n+1) is not strictly on SO(n) and should be orthonormalized to X R before further computation: [START_REF] Forbes | Linear-Matrix-Inequality-Based Solution to Wahba's Problem[END_REF] which can be solved via the proposed algorithm by forming the following vector pairs [START_REF] Bar-Itzhack | New Method for Extracting the Quaternion from a Rotation Matrix[END_REF], [START_REF] Wu | Optimal Continuous Unit Quaternions from Rotation Matrices[END_REF], with column vectors of R T ,SO(n+1) being

arg max X R ∈SO(n+1) tr X R R ⊤ T ,SO(n+1)
           b 1 = R T ,SO(n+1),1 b 2 = R T ,SO(n+1),2 . . . b n = R T ,SO(n+1),n ,              r 1 = (1, 0, . . . , 0) ⊤ r 2 = (0, 1, . . . , 0) ⊤ . . . r n = (0, 0, . . . , 1) ⊤ (50)
R T ,SO(n+1) = R T ,SO(n+1),1 , R T ,SO(n+1),2 , . . . , R T ,SO(n+1),n . (51)
The covariance of X R is then given by the [START_REF] Massey | Cross Products of Vectors in Higher Dimensional Euclidean Spaces[END_REF]. For a series of mapped SO(n + 1) measurements mapped from SE(n) X R,1 , X R,1 , . . . , X R,k , with their timestamps τ 1 , τ 2 , . . . , τ k , we can interpolate between the time instants k -1 and k by arg max

X R,k-1|k ∈SO(n+1) tr X R,k-1|k [ϱX R,k-1 + (1 -ϱ) X R,k ] ⊤ ( 52
)
where ϱ is the relative weight such that

ϱ = τ k -τ k-1|k τ k -τ k-1 ∈ (0, 1) (53)
Likewise, by setting

(b 1 , b 2 , • • • , b n ) = [ϱX R,k-1 + (1 -ϱ) X R,k ]              r 1 = (1, 0, . . . , 0) ⊤ r 2 = (0, 1, . . . , 0) ⊤ . . . r n = (0, 0, . . . , 1) ⊤ (54)
one is able to compute the interpolated result on SO(n + 1).

The covariance of X R,k-1|k can also be calculated with [START_REF] Massey | Cross Products of Vectors in Higher Dimensional Euclidean Spaces[END_REF]. Then X R,k-1|k should be inversed from SO(n + 1) back to SE(n) by the mapping F -1 . The rest steps for generating covariances of the rotational and translational parts can be referred to [START_REF] Wu | Hand-eye Calibration: 4-D Procrustes Analysis Approach[END_REF].

We use the UR10 robotic arm to perform a generated series of motion for over 150s while an Intel Realsense D435i camera is attached to the end-effector of this arm (see Fig. 3). A standard 12 × 9 chessboard is performed in the experimental environment for distinct feature extraction. There are inertial and visual outputs from the D435i but it has low frequency for the fisheye camera (30 fps). Therefore, a visual-inertial navigation system is employed to estimate the motion of the end-effector. Note that the hand-eye calibration between the installation frame of the end-effector and the sensor frame of the Realsense camera has been performed before experimentation so that the reference transformation from the readings of robotic arm can be mapped precisely into the camera frame [START_REF] Wu | Hand-eye Calibration: 4-D Procrustes Analysis Approach[END_REF]. The 1σ uncertainty of the camera measurement is set to be within 1 pixel while the in-run covariances of the inertial measurement unit (IMU) are

Σ gyro = 1.34 × 10 -5 I rad 2 Σ accel = 2.02 × 10 -3 I (m/s 2 ) 2 , (55)
where gyro and accel denote the gyroscope and accelerometer respectively and the covariances are produced according to a live Allan variance test for 1 hour [START_REF] El-Sheimy | Analysis and Modeling of MEMS based Inertial Sensors Using Allan Variance[END_REF]. For each visual measurement, the visual-inertial state estimator computes the state estimation along with its covariance by the IMU preintegration [START_REF] Forster | On-Manifold Preintegration for Real-time Visual -Inertial Odometry[END_REF]. These results have low sampling frequencies so they are interpolated uniformly to 100Hz so the reference information provided by the robotic arm could be compared with. The proposed algorithm is invoked to conduct such interpolation on SE(3), which follows the equations listed in this sub-section. The interpolated results along with their 3σ bounds are depicted in Fig. 4 and5. In Fig. 4, ξ = (ξ x , ξ y , ξ z ) ⊤ denotes the eigenaxis of a 3 × 3 rotation matrix R so that exp(ξ × ) = R and Rξ = ξ. The covariance Σ R actually reflects the uncertainty of ξ [START_REF] Nguyen | On the Covariance of X in AX = XB[END_REF].The translation in this case is in the 3-D form such that t = (t x , t y , t z ) ⊤ . From the supplementary material, we are able to observe that the robotic manipulator mainly moves along its x and z axes. It is observed that using the proposed method for the interpolation on SE(3), we are able to recover the original motion into a more dense one without loss of probabilistic propagation. This is reflected in Fig. 4 and 5 that all these interpolated results are well within the 3σ bounds. As we uses the mapping from SE(3) to SO(4) for such interpolation, the proposed method is validated to be reliable and robust on SO(4) and can achieve further state estimation tasks on SO(n) requiring uncertainty descriptions.

D. Application: Covariance-Aided Lidar Mapping

We use the KITTI dataset kitti_2011_10_03_drive_ 0027_synced [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF] to demonstrate covariance-aided Lidar mapping using the results from the proposed method. The KITTI dataset contains some data from a 64-beam Velodyne HDL-64E Lidar. The Lidar has the measurement specification of 0.09 degree angular resolution, 2 cm distance accuracy, which can be used for setting the measurement covariance for further propagation. We evaluate the trajectory errors by comparing the results with the ground-truth data in KITTI dataset. Since the entire trajectory is lengthy, we only visualize the first 125s results in Fig. 6. In these results, the frameto-frame matching is conducted via formula in [START_REF] Umeyama | Least-Squares Estimation of Transformation Parameters Between Two Point Patterns[END_REF]. The covariance results of rotation and translation are then generated and transferred to a further pose graph optimization (PGO). Here, we select the SE-Sync method [START_REF] Rosen | SE-Sync: A Certifiably Correct Algorithm for Synchronization over the Special Euclidean Group[END_REF] for globally optimal PGO computation. Therefore, the numerical computation of PGO is guaranteed to be optimal. Since successive frames are close to each other, the correspondence can be easily found by the random sample consensus (RANSAC), starting from an identity pose. Thus, we do not consider the uncertainty of the point correspondences in this work, i.e, it will be regarded as deterministic and only pose covariance in ( 40) and (41) will be taken into account. We select two representatives dealing with covariance of ICP, i.e., works in [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF] and [START_REF] Censi | An Accurate Closed-Form Estimate of ICP' s Covariance[END_REF], to make comparisons. The trajectory root mean squared errors (RMSEs) are shown in Table VIII. The best candidate in each direction is marked bold in Table VIII. It can be observed that although Censi's method outperforms the proposed method on Y-axis, the overall trajectory accuracy considering XYZ-axes of the proposed method is the best. The reason is that both [START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF] and [START_REF] Censi | An Accurate Closed-Form Estimate of ICP' s Covariance[END_REF] estimate the covariance of ICP by taking the Hessian and approximates the covariance of the least-square in first-order, which will inevitably bring about accuracy loss. The proposed method does not need any numerical optimization and is explicit so the closed-form covariance can be derived in an analytical manner as well. These analytical forms give the rise to the covariance accuracy, which has been indirectly revealed in the accuracy of the localization after PGO.

IV. CONCLUSION

The n-dimensional rigid registration problem is revisited in this paper. It is shown that using the Caylay transformation, we are able to establish a linear framework for computing the fundamental parameters. Related covariance analysis of these parameters along with recovered rotation and translation can be conducted flexibly due to the existence of the proposed linear solution. It is verified that the proposed method is slower than SVD in very high dimensions (n > 100) for a modern computer but SVD can not provide probabilistic information showing that the proposed algorithm can well handle the highdimensional rotation orthonormalization and interpolation with uncertainty descriptions. The current drawback of the proposed method is evident that it consumes too many computational resources for cases with very high dimensions. Future efforts should be paid to seek a more computationally efficient numerical framework for fast inverse or pseudo inverse of arbitrary matrices. Source codes of this paper will be made openaccess on https://github.com/zarathustr/GLnR. The video of this work is presented on https://youtu.be/BwfjQ9ZAyl4.

Fig. 1 .

 1 Fig. 1. Time complexity performances of various algorithms.

Fig. 2 .

 2 Fig. 2. The ratios of computation time from SVD, LMI and the proposed method.

Fig. 3 .

 3 Fig. 3. The generated motion of the UR10 robotic arm.

Fig. 4 .

 4 Fig. 4. The interpolated results and their 3σ bounds for the eigenaxis ξ.

Fig. 5 .

 5 Fig. 5. The interpolated results and their 3σ bounds for the translation t.

  Fig. 6. The mapping result of the proposed Lidar registration algorithm using first 125s of the KITTI dataset kitti_2011_10_03_drive_0027_synced [62]. The blue line denotes the vehicle trajectory. The blue and green points denotes the local map while red points denotes the matching result. The white points represent the reconstructed dense map after covariance-aided pose graph optimization.

  Proposed t SVD /t Proposed t LMI /t Proposed 5 5.09175700 × 10 -03 sec 2.95878440 × 10 -02 sec 7.14850000 × 10 -05 sec 7.12283276 × 10 +01 4.13902833 × 10 +02 10 1.86192880 × 10 -02 sec 3.57405530 × 10 -02 sec 1.78072000 × 10 -04 sec 1.04560447 × 10 +02 2.00708438 × 10 +02 15 2.91524970 × 10 -02 sec 5.29616330 × 10 -02 sec 2.34585000 × 10 -04 sec 1.24272639 × 10 +02 2.25767347 × 10 +02 20 5.16656850 × 10 -02 sec 2.46701489 × 10 -01 sec 1.11337100 × 10 -03 sec 4.64047339 × 10 +01 2.21580667 × 10 +02 25 7.20797970 × 10 -02 sec 7.83117401 × 10 -01 sec 8.61778000 × 10 -04 sec 8.36407950 × 10 +01 9.08722897 × 10 +02 30 1.10495224 × 10 -01 sec 2.40003185 × 10 +00 sec 2.08869100 × 10 -03 sec 5.29016614 × 10 +01 1.14906027 × 10 +03 35 1.51141937 × 10 -01 sec 6.04819777 × 10 +00 sec 4.54621700 × 10 -03 sec 3.32456495 × 10 +01 1.33038035 × 10 +03 40 2.03090954 × 10 -01 sec 1.52671507 × 10 +01 sec 8.92617900 × 10 -03 sec 2.27522834 × 10 +01 1.71037918 × 10 +03 45 2.56202188 × 10 -01 sec 3.24307563 × 10 +01 sec 2.01093880 × 10 -02 sec 1.27404269 × 10 +01 1.61271722 × 10 +03 50 3.12731122 × 10 -01 sec 6.00155415 × 10 +01 sec 3.10260630 × 10 -02 sec 1.00796263 × 10 +01 1.93435891 × 10 +03 55 3.81157892 × 10 -01 sec 1.04683072 × 10 +02 sec 4.47886660 × 10 -02 sec 8.51014165 × 10 +00 2.33726701 × 10 +03 60 4.78917751 × 10 -01 sec 1.94233782 × 10 +02 sec 8.93274220 × 10 -02 sec 5.36137437 × 10 +00 2.17440264 × 10 +03 65 5.47775943 × 10 -01 sec 3.12991523 × 10 +02 sec 1.12696783 × 10 -01 sec 4.86061739 × 10 +00 2.77728888 × 10 +03 70 6.46396852 × 10 -01 sec 4.61347369 × 10 +02 sec 1.55126971 × 10 -01 sec 4.16688889 × 10 +00 2.97399843 × 10 +03 75 7.63718827 × 10 -01 sec 7.12493501 × 10 +02 sec 2.41905543 × 10 -01 sec 3.15709519 × 10 +00 2.94533764 × 10 +03 80 8.53510834 × 10 -01 sec 1.10820564 × 10 +03 sec 3.26491385 × 10 -01 sec 2.61419098 × 10 +00 3.39428753 × 10 +03 85 1.00388324 × 10 +00 sec 1.68530646 × 10 +03 sec 4.99838141 × 10 -01 sec 2.00841664 × 10 +00 3.37170439 × 10 +03 90 1.19993136 × 10 +00 sec 2.46278443 × 10 +03 sec 6.58335981 × 10 -01 sec 1.82267322 × 10 +00 3.74092333 × 10

			TABLE VII
			COMPUTATION TIME
	Dimension n	t SVD	t LMI	t

+03 95 1.27744737 × 10 +00 sec 3.06590801 × 10 +03 sec 8.61907923 × 10 -01 sec 1.48211582 × 10 +00 3.55711779 × 10 +03 100 1.40629819 × 10 +00 sec 4.35698895 × 10 +03 sec 1.14776548 × 10 +00 sec 1.22524872 × 10 +00 3.79606203 × 10 +03

TABLE VIII RMSES

 VIII OF THE ESTIMATED TRAJECTORIES

	Direction Barczyk et al. [37]	Censi [64]	Proposed
	X	2.472284 m	2.536159 m	2.421336 m
	Y	2.034533 m	2.007825 m	2.016879 m
	Z	1.9824760 m	1.992351 m	1.972403 m

ACKNOWLEDGMENT

This research was initiated in Late 2017 and has been supported by Shenzhen Science, Technology and Innovation Comission (SZSTI) JCYJ20160401100022706, and in part by National Natural Science Foundation of China under the grants of No. U1713211. We also would like to thank Prof. Xiao-Jun Wu from Jiangnan University for providing some platforms for experimentation.

This research was supported by Shenzhen Science, Technology and Innovation Comission (SZSTI) JCYJ20160401100022706, in part by National Natural Science Foundation of China under the grants of No. U1713211. Recommended by Associate Editor X X.


Xiang Dai
email: xiang.dai@gipsa-lab.grenoble-inp.fr

Hassen Fourati
email: hassen.fourati@gipsa-lab.grenoble-inp.fr

Christophe Prieur
email: christophe.prieur@gipsa-lab.fr

A Dynamic Grid-based Q-learning for Noise Covariance Adaptation in EKF and its Application in Navigation

The process and measurement noise covariance matrices significantly impact the Extended Kalman Filter (EKF) performance and are often hand-tuned in practice, which usually entails a tedious task. Q-learning, a wellknown method in reinforcement learning, has been applied recently to better adapt the noise covariance matrices for the EKF, thanks to its simplicity and capability in handling uncertain environments. Typically, some heuristics are involved in designing the Q-learning-based EKF (QLEKF), such as tuning grid size and covariance matrices values of each state, which inevitably degrades the estimation performance when the heuristics are not suitable. We propose a dynamic grid-based Qlearning EKF (DG-QLEKF) to overcome that drawback, which brings two novelties, an updated ϵ-greedy algorithm and a dynamic grid strategy. The proposed algorithm and strategy can thoroughly exploit arbitrary search scope and find appropriate values of noise covariance matrices. The effectiveness of DG-QLEKF, applied in navigation for attitude and bias estimation, is validated through the Monte Carlo method and real flight data from an unmanned aerial vehicle. The DG-QLEKF leads to much more improved state estimation than the QLEKF and traditional EKF.

I. INTRODUCTION

The Kalman filter (KF) is known to be optimal in minimizing the estimated error covariance [START_REF] Welch | An introduction to the Kalman filter[END_REF], and in separating stochastic noises from real signals given sensors measurements for linear systems [START_REF] Brown | Introduction to random signals and applied Kalman filtering : With MATLAB exercises and solutions[END_REF]. For systems with nonlinear dynamic models, the EKF linearizes dynamics and output functions at the current state estimate to propagate predictorcorrector functions of KF. To date, the EKF has been extensively used in state estimation under noisy data and applied to engineering domains, e.g., navigation [START_REF] Zmitri | BiLSTM network-based extended kalman filter for magnetic field gradient aided indoor navigation[END_REF], robotics, computer vision [START_REF] Crassidis | Survey of nonlinear attitude estimation methods[END_REF], and electrical power systems [START_REF] Auger | Industrial applications of the Kalman filter: A review[END_REF]. It is well known that the process and measurement noise covariance matrices significantly impact the EKF performance. In the absence of exact statistical knowledge on noises, assigning them with appropriate values often requires a tedious task. In practice, the noise covariance matrices are commonly hand-tuned in the EKF through trials and errors or defined by empirical rules, which can sometimes be time-consuming or results in barely satisfactory performance.

As one of the most important reinforcement learning methods, Q-learning [START_REF] Watkins | Learning from delayed rewards[END_REF] has drawn increasing interest in adapting the noise covariance matrices of EKF [START_REF] Maoudj | Q-learning-based navigation for mobile robots in continuous and dynamic environments[END_REF], [START_REF] Xiong | Q-learning for noise covariance adaptation in extended KALMAN filter[END_REF], for its modelfree algorithm, low computation demand, and capability in achieving optimality in Markov decision processes [START_REF] Watkins | Q-learning[END_REF]. In our previous work [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF], a Q-learning-based EKF (QLEKF) is proposed to autonomously adapt the values of process and measurement noise covariance matrices in the attitude estimation of a rigid body. Though improvement is revealed in estimation errors compared to the traditional EKF using hand-tuned noise covariance matrices, the design of QLEKF involves certain amounts of heuristics and a rule of thumb.

This paper proposes a dynamic grid-based Q-learning EKF (DG-QLEKF) to address the deficiencies mentioned above in QLEKF. To begin with, we propose an updated ϵ-greedy algorithm that enables unbiased exploration of state-action space and non-worse state convergence w.r.t. the reference state. Next, we propose the dynamic 3-by-3 grid determined by the dynamic center and ratio, where the center tracks the best-found noise covariance matrices, and the ratio defines the searching scope. In the continuation, we introduce the absolute accumulative innovation term as the criterion to quantify the performance of each process and measurement covariance matrices pair.

The main contributions of the paper are threefold. First, we propose an updated ϵ-greedy algorithm suited for optimal value searching in the Q-learning context. Second, we propose an efficient dynamic grid strategy for Q-learning with adaptable update characteristics that save the effort in defining the grid. Third, we propose an easy-to-implement and quick-responsive Q-learning algorithm that can adapt the noise covariance matrices of the EKF for large scope and with fine precision.

The rest of the paper is organized as follows. Section II formulates the state-space dynamic functions for nonlinear systems and introduces the traditional EKF. Section III recalls some preliminaries on the Q-learning and QLEKF. Section IV details the dynamic grid strategy and the updated ϵ-greedy algorithm, followed by the overall DG-QLEKF. Monte Carlo simulations are performed in Section V to estimate the attitude of a rigid body and sensor biases using the DG-QLEKF. Conclusion and future works are given in Section VI.

II. STATE-SPACE REPRESENTATION AND EKF-BASED ESTIMATION ALGORITHM

In this section, we formulate the dynamic functions of the nonlinear system and introduce the traditional EKF to estimate the observable states.

A. System formulation

We consider a discrete Markov model to describe a nonlinear system, with two dynamic functions as follows

x k+1 = f (x k ) + w k , y k = g(x k ) + v k , (1) (2)
where f (•) and g(•) are process and measurement dynamic functions, respectively, and are assumed to be continuously differentiable, x k is the system state to be estimated, y k is the system measurable output, w k and v k are the process and measurement noises, respectively.

We assume that the process and measurement noises are independent Gaussian with zero mean, which means for ∀ i, j = 0, 1, 2, ..., we have

E[w i ] = 0, E[v i ] = 0, E[w i w T i ] = Q, E[v i v T i ] = R, E[w i v T j ] = 0, for i ̸ = j: E[w i w T j ] = 0 and E[v i v T j ] = 0,
where Q and R are the process and measurement noises covariance matrix, respectively.

B. The traditional extended Kalman filter

For nonlinear systems expressed by ( 1) and ( 2), the EKF used for state estimation is summarized in Alg. 1.

Algorithm 1 Traditional EKF 1: Input xk|k , P k|k , y k , Q, R 2: xk+1|k = f ( xk|k , 0) 3: A k = ∂f ∂x ( xk|k ) 4: P k+1|k = A k P k|k A T k + Q 5: C k+1 = ∂g ∂x ( xk+1|k ) 6: K k+1 = P k+1|k C T k+1 (C k+1 P k+1|k C T k+1 + R) -1 7: ỹk+1 = y k+1 -g( xk+1|k , 0) 8: xk+1|k+1 = xk+1|k + K k+1 ỹk+1 9: P k+1|k+1 = P k+1|k -K k+1 C k+1 P k+1|k 10: Output xk+1|k+1 , P k+1|k+1 , ỹk+1 III. THE Q-LEARNING-BASED APPROACH FOR NOISE COVARIANCE ADAPTATION
In this section, the Q-learning basics and their combination with the EKF are introduced, and then our previous work [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF] on Q-learning-based adaptation is recalled.

A. Preliminaries on Q-learning Q-learning is a reinforcement learning method that maximizes the long-term reward in a multi-state environment. That environment typically consists of discrete state-action pairs, each assigned with a scalar called Q-value. In Qlearning, the agent attempts to obtain an optimal sequential actions decision by maximizing the Q-value of each stateaction pair, called exploitation. Visiting the non-exploited state-action space by the agent is called exploration. Usually, exploitation and exploration need to be balanced by an action strategy. The ϵ-greedy algorithm [START_REF] Sutton | Reinforcement learning: An introduction[END_REF] is adopted in this work for its implementation simplicity and effectiveness compared to other exploration methods, e.g., random walk exploration [START_REF] Thrun | Efficient exploration in reinforcement learning[END_REF], [START_REF] Whitehead | A complexity analysis of cooperative mechanisms in reinforcement learning[END_REF], and Softmax action selection method [START_REF] Bridle | Training stochastic model recognition algorithms as networks can lead to maximum mutual information estimation of parameters[END_REF]. In the ϵ-greedy algorithm, the agent chooses a random action with a predefined probability of ϵ or picks the action that maximizes the Q-value with the probability of 1 -ϵ.

Each time after executing an action a, the agent receives a response from the environment, which is translated to a reward (R), showing how good the action is. The cumulative reward is stored as Q-value

Q(s, a) = (1 -α)Q(s, a) + α[R + γ max a∈A(s) Q(s ′ , a)], (3)
where Q(s, a) ∈ R is the Q-value for the action a in state s, R ∈ R is the reward gained by executing a in state s, α is the learning rate, γ is the discount factor, and A(s) is the possible actions set when the agent is at s.

B. Q-learning-based noise covariance adaptation approach

In the Q-learning method, an agent typically moves among discrete states in a grid. For example, we can set the noise covariance matrices to M different values for Q as {Q (1) , ..., Q (M ) }, and N different values for R as {R (1) , ..., R (N ) }, and place them in a M -by-N grid, of which the element (i, j) stands for the noise covariance matrices (Q (i) , R (j) ). In this way, (Q (i) , R (j) ) are used in the EKF when the agent is at state (i, j).

In [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF], we have proposed the QLEKF that runs three parallel EKFs at each time step: the traditional EKF, which sets some initial values of (Q, R) for all time steps and serves as the benchmark for Q-learning; the learning EKF, which searches appropriate noise covariance matrices from the grid by the Q-learning algorithm; and the learned EKF, which outputs the result of estimation according to the covariance matrices found by the learning EKF. Please refer to Alg. 2 in [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF] for more details about the QLEKF. As shown in [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF], the QLEKF exhibits the benefit of improving the EKF state estimation by searching for more appropriate noise covariance matrices from a predefined set of values. However, QLEKF contains potential insufficiencies:

1) Heuristics are used to define (Q, R) for the traditional (reference) EKF, which matters to a great extent to the QLEKF estimation performance as it determines the reward computation.

2) The determination of the grid size and the value of each

(Q (i) , R (j)
) is generally heuristic, which is indeed a key factor for the QLEKF performance.

3) The state estimation produced by the learned EKF uses the exact sequence {(Q (i) , R (j) )} visited by the learning EKF, in which the random actions may degrade the estimation performance.

IV. THE DYNAMIC GRID-BASED Q-LEARNING ALGORITHM APPLIED TO THE EKF

To overcome the potential drawbacks of the QLEKF listed in Subsection III-B, we propose a deterministic way to design the dynamic grid and an update of the ϵ-greedy algorithm.

A. The dynamic grid and updated ϵ-greedy algorithm

We propose an advanced variant of the ϵ-greedy algorithm and the way to build the dynamic grid, shown in Alg. 2. First, Steps 4-5 enable an unbiased exploration at state s when its possible actions possess the same Q-value. Otherwise, if exploitation (Step 7) is enforced in that case, the agent would choose the first action located in A(s) by default, which causes a biased exploration. Second, Steps 8-9 ensure that only the states with positive reward can be chosen to stay, which guarantees the dominance of noise covariance matrices visited in Q-learning over their reference values. In what follows, we propose a dynamic grid strategy to address the difficulty in determining the grid size and state values of the QLEKF. Let (Q 0 , R 0 ) denote the initial noise covariance matrices used for the traditional EKF. We consider that the adaptation of noise covariance matrices

Algorithm 2 Updated ϵ-greedy Algorithm 1: a = Updated ϵ-greedy(ϵ, s, A(s), Q(s, a)) 2: Input: s, A(s) and Q(s, a) 3: Generate n from uniform distribution n ∼ U (0, 1) 4: if n < ϵ or ∀a 1 , a 2 ∈ A(s), Q(s, a 1 ) = Q(s,
(Q, R) is carried out in a set as C = {(Q, R) | Q = q 0 Q 0 , R = r 0 R 0 , 0 < q ≤ q 0 ≤ q, 0 < r ≤ r 0 ≤ r},
where q 0 and r 0 are the multiplication ratio associated with Q 0 and R 0 , respectively. The dynamic grid shown in Fig. 1 is created in the following way: i), determine

(Q c , R c ) ∈ C for the center element; ii), compute Q = max{q, 1 q }Q c , Q = min{q, q}Q c , R = max{r, 1
r }R c , R = min{r, r}R c where q > 1 and r > 1 are the multiplication factors associated with Q c and R c , respectively.

We design the 3-by-3 dynamic grid for the following reasons. First, 3-by-3 is the smallest grid that contains the noise covariance matrices combinations of reference values Q c , R c and their greater and smaller counterparts: R, R, Q and Q. Second, it is sufficient for Q-learning to explore and exploit all directions from the center element. Third, its all 9 elements can be visited whit a shorter learning period, compared to larger size grids in [START_REF] Xiong | Q-learning for noise covariance adaptation in extended KALMAN filter[END_REF], [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF]. Fourth, by dynamically updating the ratio r, q and the central element (Q c , R c ), it allows Q-learning to search in an arbitrarily large scope (by manipulating q, r, r and q) with an arbitrarily high precision (by setting r and q close to 1).

B. The Dynamic Grid-based Q-learning EKF approach

The DG-QLEKF is summarized in Alg. 3, in which Step 18 integrates Alg. 4 to update the 3-by-3 dynamic grid. We emphasize the difference in contrast to the QLEKF in elaborating the DG-QLEKF. To begin with, we introduce an absolute metric T in Step 11 as the innovation term norm of the learning EKF, which eliminates the impact of the innovation term of the reference EKF and reflects the absolute innovation magnitude of each distinct (Q, R). If

Step 17 in Alg. 3 is satisfied, then we focus on T mean , the average T of the state s c as presented in Step 2 of Alg. 4, because it further mitigates the impact of previous state estimate xl k on T and indicates the average innovation term magnitude when 'stay' action is executed. If a lower T mean appears, we first reset n T , the counter of local state convergence without finding a lower T mean , to 0, making the current grid be continually examined enough times before update. Then if s c corresponds to the non-center element, we move the center to that element and reset the ratio to initial values as in Step 6, which generates a new 3-by-3 grid. If no lower T mean is observed, n T is self added by 1 until it reaches the predefined threshold n ratio , in which case we increase r and q. The logic behind this is, first, multiple times (n ratio ) check after local convergence averts the non-fully visited situation. Second, every newly found center is an appropriate values candidate. In turn, from its near (small ratio) to distant (large ratio) neighbors, its T mean should be compared with that of other candidates. Unlike in the QLEKF, where the traditional (reference) EKF uses fixed noise covariance matrices, in the DG-QLEKF, each time (Q c , R c ) is changed (lower T min is found), it will be applied to the traditional EKF from the next period learning, ensuring that the best-found noise covariance matrices are referred to compute the reward R in Step 10 of Alg. 3. After the Q-learning has searched throughout the predefined scope (Step 23 of Alg. 3), the noise covariance matrices (Q c , R c ) that produce the lowest average innovation term, are used as deterministic values of noise covariance matrices to compute the final state estimation.

Algorithm 3 Dynamic Grid Q-learning Extended Kalman Filter (DG-QLEKF) 1: Initialize xt 0|0 = xl 0|0 , P t 0|0 = P l 0|0 , r, r 0 , q, r, q, q 0 , R 0 , Q 0 , n l and n ratio 2: R ← 0, T ← 0,

T record ← ∅, T min ← ∞, k ← 0, R c ← R 0 , Q c ← Q 0 , r ← r 0 , q ← q 0 , n T ← 0 3: ∀s ∈ S, a ∈ A(s), Q(s, a) ← 0 4: repeat 5: a = Updated ϵ-greedy(0.1, s, A(s), Q(s, a)) 6:
Execute action a and obtain state s ′

7:

for each time step in one period do 8: end if 23: until q = q, r = r and n T = n ratio 24: use Q c and R c to compute { xk } and {P k }

[ xt k+1 , P t k+1 , ỹt k+1 ] = EKF ( xt k , P t k , y t k , Q c , R c ) {Reference EKF} 9: [ xl k+1 , P l k+1 , ỹl k+1 ] = EKF ( xl k , P l k , y l k , Q(s ′ ), R(s ′ ))
T record ← [T record , T ], s ← s ′ , R ← 0, T ←

C. Estimation error bounds of DG-QLEKF

In this subsection, we prove that the mean square of the state estimation error of DG-QLEKF is exponentially bounded.

According to Alg. 1, ( 1) and ( 2) can be expanded at xk xk+1|k using Taylor approximation as [START_REF] Auger | Industrial applications of the Kalman filter: A review[END_REF] where ϕ(•) and ψ(•) are nonlinear functions to offset the linearization error of f (•) and g(•), respectively.

x k+1 =f ( xk ) + A k (x k -xk ) + w k + ϕ(x k , xk ), y k+1 =g( xk+1|k ) + C k (x k+1 -xk+1|k ) + v k + ψ(x k+1 , xk+1|k ), (4)
Let the state estimation error is defined by xk = x k -xk . Combining Step 2, 7, 8 of Alg. 1, (4) and ( 5), we have

xk+1 = (I -K k+1 C k+1 )A k xk + r k + s k , (6)
1 In this case, we consider max s∈S,a∈A(s) Q(s, a) = Q(sc, 'stay') is practically satisfied, e.g. the state trajectory is locally converged to sc. To balance the efficiency and accuracy, The condition that sc appears at least 8 times in the least 10 consecutive states is adopted for simulations in Section V.

Algorithm 4 Dynamic Grid Update (DGU) 1: Input: s c , T record , q, r, T min , n l and the current 3-by-3 grid 2: T mean ← mean value of T corresponding to s c over the latest n l elements of T record 3: if T mean < T min {better innovation found for converged state} then r ← r 0 , q ← q 0 {reset the ratio} 8:

T min ← T mean 9:

end if 10: else 11:

n T ← n T + 1 12:
if n T = n ratio then 13: increase r and q {current grid is well exploited, expand search scope} 14:

end if 15: end if where r k = (I -K k+1 C k+1 )ϕ(x k , xk ) - K k+1 ψ(x k+1 , xk+1|k ), s k = (I -K k+1 C k+1 )w k - K k+1 v k+1 .
Theorem 1: Consider a nonlinear stochastic system given by (1), [START_REF] Brown | Introduction to random signals and applied Kalman filtering : With MATLAB exercises and solutions[END_REF]. If there are real numbers a, a, c, c, p, p, q ′ , q ′ , r ′ , r ′ , α 1 , α 2 , α 3 > 0, such that for every k ≥ 0 we have

a ≤ ||A k || ≤ a, c ≤ ||C k || ≤ c, || xk || ≤ α 1 , pI ≤ P k ≤ pI, q ′ I ≤ Q c ≤ q ′ I, r ′ I ≤ R c ≤ r ′ I, ||ϕ(x k , xk )|| ≤ α 2 || xk || 2 , ||ψ(x k+1 , xk+1|k )|| ≤ α 3 || xk || 2 , (7) (8) (9) (10)
then we can design a ϵ 0 > 0 provided that || x0 || ≤ ϵ 0 such that the estimation error xk of DG-QLEKF represented by Alg. 3 is bounded in mean square and bounded with probability one.

V. NUMERICAL SIMULATIONS WITH REAL DATA

We consider the framework of navigation systems and focus on the problem of attitude/bias estimation using magnetic, angular rate, and gravity (MARG) sensors. The proposed estimation approach is evaluated with 50 Monte Carlo simulations using real flight data from Euroc database [START_REF] Burri | The EuRoC micro aerial vehicle datasets[END_REF].

A. Dynamic model formulation for attitude and bias

We build up the dynamic model for a rigid body moving in space. Let B denote the body frame and N denote the navigation frame. First, we introduce the dynamic of attitude as q = 1 2 Ω(ω)q, where q = [q 0 , q 1 , q 2 , q 3 ] T is the quaternion that describes the attitude with ∥q∥ = 1, of which q 0 is the scalar part and q 1 , q 2 , q 3 compose the vector part, ω = [ω x , ω y , ω z ] T ∈ R 3 is the true angular velocity of B w.r.t. N expressed in N , and

Ω(ω) = -⌊ω×⌋ ω -ω T 0 , ⌊ω×⌋ =   0 -ω z ω y ω z 0 -ω x -ω y ω x 0   .
Second, C B N (q) ∈ R 3×3 is the rotation matrix from N to B expressed in B using quaternion elements [START_REF] Kuipers | Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace, and Virtual Reality[END_REF],

C B N (q) =   2q 2 0 -1 + 2q 2 1
2 (q 1 q 2 + q 0 q 3 ) 2 (q 1 q 3 -q 0 q 2 ) 2 (q 1 q 2 -q 0 q 3 ) 2q 2 0 -1 + 2q 2 2 2 (q 2 q 3 + q 0 q 1 ) 2 (q 1 q 3 + q 0 q 2 ) 2 (q 2 q 3 -q 0 q 1 ) 2q

2 0 -1 + 2q 2 3   .
Then, the output of MARG sensors can be modeled 2 by

     ω m = ω + b g + v g , a = C b n (q)g + b a + v a , m = C b n (q)h + b m + v m , (11)
where ω m ∈ R Next, we consider the state vector to be estimated as

x k = [q k ; b g k ; b a k ; b m k ].
After discretization, the state transition equation is formulated as

x k+1 = f (x k ) + w k = blkdiag(Ω k , B, I 3 , I 3 )[q k ; b g k ; b a k ; b m k ] + [w q k ; w g k ; w a k ; w m k ],
where Ω k = I 3 + 1 2 Ω(ω m k -b g k )T e is the quaternion dynamic matrix [START_REF] Dai | Q-learning-based noise covariance adaptation in Kalman filter for MARG sensors attitude estimation[END_REF], [START_REF] Wertz | Spacecraft Attitude Determination and Control[END_REF], and the gyroscope bias b g k is represented by a discrete Gaussian-Markov process, in which B = (1 -β -1 )T e I 3 is derived from ḃg k = -β -1 b g k + w g k with β being the time constant of bias variation, and T e = t k+1 -t k denotes the sampling interval, b a k and b m k are modeled as standard random walk, and w q k , w g k , w a k and w m k form the process noise, assumed to be uncorrelated Gaussian with zero mean and covariance matrices Σ q w = σ 2 w,q I 3 , Σ g w = σ 2 w,g I 3 , Σ a w = σ 2 w,a I 3 and Σ m w = σ 2 w,m I 3 . As such, the process noise covariance matrix is diagonal and is represented by Q = blkdiag(Σ q w , Σ g w , Σ a w , Σ m w ). Subsequently, the measurement model is constructed by grouping measurements of accelerometer and magnetometer:

y k = [a k ; m k ] = g(x k ) + v k = blkdiag(C b n (q k ), C b n (q k ))[g; h]+[v a k ; v m k ].
Finally, the measurement noise covariance matrix is diagonal and is represented by R = blkdiag(Σ v a , Σ v m ).

B. Monte Carlo numerical simulations

We use the ground truth of b g , b a and q true from Euroc database [START_REF] Burri | The EuRoC micro aerial vehicle datasets[END_REF]. And q true is used to solve out the ω. The 2 In the simulation, we consider that the rigid body is just rotating on itself and its center of mass is not moving; thus the body linear acceleration expressed in N is zero. k } are set as a geometric progression with a ratio of 10 and Q (3) = Q 0 , R (3) = R 0 . The Q-learning search starts at the grid (Q (2) , R (2) ). For each Monte Carlo simulation, we run 360 periods and 100 time steps for each iteration. The learning rate, discount factor and random action selection probability are fixed to α = 0.1, γ = 0.9 and ϵ = 0.1, respectively.

Table I shows that after the convergence of 50 Monte Carlo simulations, the three EKFs deliver different state estimation performances. Generally, the QLEKF outperforms the traditional EKF, particularly improving the quaternion error by 58.34%. As for the DG-QLEKF, it exhibits a prominent predominance of all statistics compared to both the traditional EKF and QLEKF. In detail, the average improvements in quaternion error, RMSEs of gyroscope bias, accelerometer bias, and magnetometer bias of DG-QLEKF compared to the traditional EKF are 88.93%, 40.60%, 82.39% and 74.62%, while those improvements of DG-QLEKF compared to QLEKF are 73.42%, 29.70%, 75.21%, and 69.22%, respectively. As shown in Fig. 2, the three curves share similar tendencies and fluctuations after convergence. In contrast, the quaternion error of DG-QLEKF is always distinctly lower than the other two EKFs in the whole convergence period. Similar behaviors can be observed from Fig. 3, but with more unstable variations of QLEKF. * For a ground truth unit quaternion qtrue, the quaternion error of its unit estimate q is computed as ∥q -1 true ⊗ qq I ∥, where q I = [1 0 0 0] T is the identity quaternion. In this paper, the Q-learning-based EKF (QLEKF) for state estimation of nonlinear systems was introduced as a premise to address the often-cumbersome tuning of noise covariance matrices in the EKF. To overcome the existing drawbacks of QLEKF resulting from heuristics in designing Q-learning, a dynamic grid-based Q-learning EKF algorithm (DG-QLEKF) has been proposed, which can thoroughly exploit an arbitrary search scope to find appropriate values of noise covariance matrices, leading to excellent state estimation. Through Monte Carlo numerical simulations based on real flight data from an unmanned aerial vehicle, the DG-QLEKF, on average, has revealed much more improvement in attitude and biases estimation after convergence compared to the traditional EKF and QLEKF. Future work on the DG-QLEKF can be undertaken by adapting inter-process and inter-measurement noises covariance matrices.

Fig. 1 :

 1 Fig.1: The 3-by-3 dynamic grid of states (process and measurement noises covariance matrices pairs) and the corresponding possible actions, represented by arrows. The agent is allowed to 'stay' at its current state or move to its adjacent state by executing one action.

4 :n 5 :

 45 T ← 0 {reset no-new-better counter} if s c ̸ = (2, 2) {new center is found} then 6: Q c , R c ← noise covariance matrices values in the current 3-by-3 grid corresponding to s c 7:

  3 is the measured angular velocity, m ∈ R 3 is the measured Earth's magnetic field in B w.r.t. N , a ∈ R 3 is the measured acceleration in B w.r.t. N , g ∈ R 3 is the gravity vector, h ∈ R 3 is the Earth's magnetic field in N , b g , b a , b m ∈ R 3 are the bias of gyroscope, accelerometer and magnetometer, respectively, and v g , v a , v m ∈ R 3 are assumed to be uncorrelated Gaussian noises of these sensors with zero mean and covariance matrices Σ g = σ 2 g I 3 , Σ a = σ 2 a I 3 and Σ m = σ 2 m I 3 , where I 3 denotes the identity matrix of dimension 3.

Fig. 2 :

 2 Fig. 2: Mean quaternion error (refer to Table I for the computation) among the traditional EKF, QLEKF, and DG-QLEKF after convergence of 50 Monte Carlo simulations

Fig. 3 :

 3 Fig. 3: Norm of RMSE of gyroscope bias, accelerometer bias, and magnetometer bias among the traditional EKF, QLEKF, and DG-QLEKF after convergence of 50 Monte Carlo simulations

  a 2 ) then

	5:	select a randomly from A(s)
	6: else
	7:	a = arg max a∈A(s) Q(s, a)
	8:	if a='stay' and Q(s, 'stay') < 0 then
	9:	select action a randomly from A(s) \ 'stay'
	10:	end if
	11: end if
		1,1	1,2	1,3
		# 𝑸𝒄 , 𝑹𝒄	# 𝑸𝒄 , 𝑹𝒄	# 𝑸𝒄 , # 𝑹𝒄
		2,1	2,2	2,3
		𝑸𝒄 , 𝑹𝒄	𝑸𝒄 , 𝑹𝒄	𝑸 𝒄 , # 𝑹 𝒄
		3,1	3,2	3,3
		𝑸 𝒄 , 𝑹 𝒄	𝑸𝒄 , 𝑹𝒄	𝑸𝒄 , # 𝑹𝒄

  0

	16:	xl k+1 ← xt k+1 , P l k+1 ← P t k+1
	17:

if a state s c appears sufficiently frequent in the latest n l consecutive states 1 then 18: Update Q c , R c , q and r by DGU (Alg. 4) 19: T record ← ∅ {reset record lists after convergence} 20: select s randomly state from S 21: ∀s ∈ S, a ∈ A(s), Q(s, a) ← 0 22:

TABLE I :

 I Mean of 50 Monte Carlo simulations after convergence for quaternion error and Root Mean Square Error (RMSE) of bias of gyroscope, accelerometer, and magnetometer

		Mean of quaternion error* (×10 -3 )	RMSE of gyroscope bias (mrad/s) x-axis y-axis z-axis	RMSE of accelerometer bias (m/s 2 ) x-axis y-axis z-axis	RMSE of magnetometer bias (mGauss) x-axis y-axis z-axis
	Traditional EKF	18.35	1.564	1.534	1.184	0.135	0.183	0.064	6.756	1.744	3.580
	QLEKF	7.644	1.231	1.116	1.133	0.120	0.067	0.048	4.942	1.089	3.963
	DG-QLEKF	2.032	0.760	0.643	1.038	0.043	0.014	0.008	1.816	0.277	1.196

This work has been partially supported by MIAI@Grenoble Alpes (ANR-19-P3IA-0003).