Constructive Synchronous Observer Design for Inertial Navigation with Delayed GNSS Measurements

—Inertial Navigation Systems (INS) estimate a vehicle’s navigation states (attitude, velocity, and position) by combining measurements from an Inertial Measurement Unit (IMU) with other supporting sensors, typically including a GNSS and a magnetometer. Recent nonlinear observer designs for INS provide powerful stability guarantees but do not account for some of the real-world challenges of INS. One of the key challenges is to account for the time-delay characteristic of GNSS measurements. This paper addresses this question by extending recent work on synchronous observer design for INS. The delayed GNSS measurements are related to the state at the current time using recursively-computable delay matrices, and this is used to design correction terms that leads to almost-globally asymptotic and locally exponential stability of the error. Simulation results verify the proposed observer and show that the compensation of time-delay is key to both transient and steady-state performance.


I. INTRODUCTION
Inertial Navigation Systems (INS) are algorithms that fuse measurements from Inertial Measurement Units (IMUs), consisting of a gyroscope and accelerometer, to estimate a vehicle's attitude, velocity, and position with respect to a fixed reference frame.Typically, INS is supported by additional sensors, including a GNSS and a magnetometer, to counteract the build-up of error resulting from integration of noisy MEMS IMU devices [1].INS solutions are a vital part of many navigation and control systems across application domains in aerospace, maritime, and robotics engineering [1], [2], [3], [4].
The industry-standard approach to INS is the multiplicative extended Kalman filter (MEKF) [5].Recently, more advanced alternatives such as the Invariant EKF [6] and Equivariant Filter (EqF) [7], [8] have been shown to significantly improve accuracy and robustness.However, this class of solutions provides only local and trajectory-dependent guarantees of convergence.Authors in the nonlinear observers community have proposed alternative solutions to the INS problem with greatly improved domains of convergence [2], [9], [3], [10], [4].Due to the nonlinearity of attitude and its coupling with the velocity and position dynamics, these observer designs typically exhibit semiglobal exponential stability rather than the almost-global stability characteristic of earlier work on attitude estimation [11], [12].Recently, the authors have developed a new approach to INS [13], [14], [15] that exploits a 'group-affine' property of the system dynamics to yield a synchronous error with almost-global P. van Goor, P. Wickramasinghe, M. Hampsey, and R. Mahony are with the Systems Theory and Robotics Group, School of Engineering, Australian National University, Australia {first name}.{lastname}@anu.edu.auasymptotic and local exponential stability [16].However, GNSS position and velocity measurements are typically delayed in time due to the cross correlation process required to extract the timing signals received from each satellite.These time-delays can cause significant errors if not compensated in an observer design [17].There is some prior work in the nonlinear observer community that addresses this issue.Khosravian et al. [18] proposed an observer-predictor approach to handle delayed measurements for mixed-invariant systems on Lie groups, building on an existing observer design to inherit its stability properties.Hansen et al. [17] extended the semiglobally exponentially stable observer proposed in [3] to consider delayed GNSS measurements by estimating the delayed state and then propagating this forward in time using stored IMU measurements.
In this paper, we consider the INS problem with magnetometer and delayed GNSS measurements.We build on the observer architecture proposed in [15] to obtain synchronous error dynamics [16], and we use a predictor structure similar to [18] to relate the delayed GNSS measurements of the position and velocity at time t − δ to the state at the current time t.Thanks to the synchrony property of the error, we are able to design separate correction terms for the delayed position and velocity measurements and easily combine them in the final design.This yields (to the authors' knowledge) the first INS solution for delayed GNSS measurements with almost-globally asymptotic and locally exponential stability of the error dynamics.Our simulation results compare the performance of an observer with and without delay compensation providing a clear demonstration of the impact of the proposed methodology.

II. PRELIMINARIES
For any ω ∈ R 3 , the skew matrix is defined as A set of time-varying vectors µ 1 (t), ..., µ n (t) ∈ R 3 is said to be persistently exciting [19] if there exist δ, T > 0 such that, for all t ≥ 0, A number of Lie groups and their Lie algebras are used throughout the paper.
The special orthogonal group: The extended special Euclidean group [20]: The extended similarity transformation group [15]:

III. PROBLEM FORMULATION
We consider the problem of estimating the navigation states of a vehicle equipped with an IMU, a GNSS, and a magnetometer.For simplicity, we will identify the bodyframe of the vehicle with the axes of the IMU.Let R ∈ SO(3), v, p ∈ R 3 be the vehicle attitude, velocity, and position, respectively, all with respect to a reference frame fixed to the Earth's surface.Let Ω ∈ R 3 denote the angular velocity measured by the gyroscope, let a ∈ R 3 denote the specific acceleration measured by the accelerometer, and let g ∈ R 3 denote the gravity vector as measured in the reference frame (typically g ≈ 9.81e 3 ms −2 ).Then the system dynamics are given by The GNSS is modelled as providing measurements of the position and velocity of the vehicle, delayed by a constant offset δ ≥ 0. The measured position y δ p and velocity y δ v at a time t are given by The superscript δ is used to emphasise the delay.The (undelayed) magnetometer measurement is given by where ẙm ∈ R 3 is the reference magnetic field direction.Our goal is to design an observer for the states at the time t using only the IMU, GNSS, and magnetometer measurements available at t.

IV. LIE GROUP INTERPRETATION
The system dynamics (2) may be interpreted as groupaffine dynamics on the extended pose group SE 2 (3) [14].Specifically, let X ∈ SE 2 (3) so that Then the system dynamics (2) may be written as where This interpretation of the dynamics leads us to the observer architecture proposed in [16], and has been applied to INS with undelayed GNSS measurements in [14], [15].The measurements may also be interpreted through the Lie group formalism.One observes that for any C ∈ R 2 .In particular, C = e 1 for velocity measurements and C = e 2 for position measurements.This matrix form of the measurements is powerful for studying the effect of time-delay in the sequel.

A. Time-Delay Matrices
In this section, we show how one can construct stateindependent delay matrices that capture the effect of a delay δ on a trajectory of the system (5).
Proof: The result is shown using the uniqueness of ODE solutions.Let X δ := X(t − δ) for a convenient shorthand.Then, for a fixed t ∈ [0, ∞) and recalling (5) on has . Thus, taking the partial derivative with respect to δ of the left-hand side should yield zero: As for the initial condition, The solutions of these time-delay matrices Y δ L (t) and Y δ R (t) can also be propagated through time.In fact, since the matrices G and N are constants, the solution to Y δ L is also constant for any δ.
Lemma 4.2: 3) be defined as in Lemma 4.1.Then, for any fixed δ ≥ 0, the left delay matrix is constant and given by for all t ∈ [0, ∞).The time-derivative of the right delay matrix is given by for all t ≥ δ.
Proof: For the left delay matrix, simply observe that the exponential exp(−δ(G + N )) is exactly the solution to the defining equation (8a), independent of t.In particular, this also means that As for the right delay matrix, it must be that ( 9) holds for all t ≥ δ by Lemma 4.1.We use the shorthand notations X δ := X(t − δ) and U δ := U (t − δ).Rewriting ( 9), one has , where the second-last line follows from (12).
Let Y δ R , Y δ L be delay matrices as defined in Lemma 4.1.Expanding the formula (10) yields The previous Lemma showed that the solution to Y δ L (t) is fixed in t and can be easily obtained for any δ.It also showed that the solution of Y δ R can be propagated through time.In practice this means that, following initialisation of Y δ R (t) for the period of t ∈ [0, δ) using (8b), the solution Y δ R (t) can be updated by recursively solving (11) for all t ≥ δ.The next Lemma shows how the delay matrices can be used to modify the GNSS measurements by using their matrix form (7). where C. Proof: Using the matrix form (7) of the measurement, Recall the delay equation ( 9) and the expanded form of Y L (13).Then multiply both sides with (Y δ L ) −1 to obtain where the last line follows from the fact that as another consequence of the delay equation (9).

V. OBSERVER DESIGN
We will use the observer architecture proposed in [14, Section 3.1], and then provide a general way to incorporate measurements of the form (14).The result of Lemma 4.3 is the key to using the delayed measurements.Recall the system dynamics (5) and consider the observer architecture [14] Ẋ where is the auxiliary state, and ∆ ∈ se 2 (3) and Γ ∈ sim 2 (3) are correction terms that we have yet to design.The observer error is defined to be and is a synchronous error [15].For further details about the rationale of this error definition, please see [15].We may simplify the auxiliary state dynamics (15b) by choosing the rotation correction Ω Γ ≡ 0 and the initial condition R Z (0) = I 3 .The result is the ṘZ ≡ 0 for all time, and thus R Z ≡ I 3 .This is possible as the uncorrected dynamics Ż = (G + N )Z do not involve the rotation component R Z .We will use this simplification through the remainder of the paper.
Let R Ē ∈ SO(3) and V Ē ∈ R 3×2 denote the rotational and translational components of Ē, respectively, then Lemma 5.1 (Synchrony [14]): The dynamics of the observer error Ē defined in (16) are In particular, they are independent of the inputs, and are zero when the correction terms ∆ and Γ are nullified.One of the useful consequences of synchrony is its application to modular observer design (cf.[15,Theorem 5.4]).This property is central to the final observer design we will propose.In the sequel, we will consider the positive-definite cost function L : SE 2 (3) → R + defined by We restate Theorem 5.4 from [15].Theorem 5.2 (Modularity Theorem): 3) be a collection of correction terms and define the component total-derivatives of L by for each i = 1, . . ., n. Suppose that L i ≤ 0 and that ∆ i and Γ i are uniformly continuous in time.Define the correction terms for some (possibly time-varying) uniformly continuous gains α i ≥ α > 0. Then the cost function L is a Lyapunov function for the dynamics of Ē, in the sense that L ≤ 0, and its set of equilibria is exactly the intersection E := ∩ n i=1 E i , where The following Lemma provides a way to choose correction terms based on any vector-type measurement of the form µ = Rμ + V C. As we have already seen in Section IV-A, the delayed GNSS position and velocity measurements are of this form with μ = 0 3×1 and µ = v, C = e 1 for velocity and µ = p, C = e 2 for position.In fact, the magnetometer measurement can also be written in the same form, with μ = y m , µ = ẙm , and C = 0 2×1 .Combining this insight with Theorem 5.2 allows us to easily create an observer that incorporates all three measurement types.
Lemma 5.3: Suppose µ ∈ R 3 is a measurement of the system state X = (R, V ) ∈ SE 2 (3) of the form where μ ∈ R 3 and C ∈ R 2 are known.Consider the observer architecture (15), the observer error (16), and the cost function (19).Let μ = Rμ + V C and µ Z = V Z A −1 Z C. Choose gain k R , k V ≥ 0 and K q ∈ S ≥0 (2), and define the correction terms by Then the cost function derivative satisfies where λ min (K q ) is the minimum eigenvalue of K q .
Proof: Expanding the error dynamics (18) into their component parts yields Substituting the chosen correction terms into (20b) yields As for the rotational component, observe that Therefore, by [15,Lemma A.1], These results combine to give the cost function derivative, This completes the proof.
Finally, we are ready to state our main theorem.This theorem combines correction terms for delayed GNSS position and velocity, and (undelayed) magnetometer measurements.
Theorem 5.4: Let X ∈ SE 2 (3) denote the system state as in ( 4), with dynamics given by ( 5).Let y δ p , y δ v , y m ∈ R 3 denote the delayed GNSS position (3), delayed GNSS velocity, and magnetometer measurements, respectively.Define to be the delay matrices as in Lemma 4.1, and define Define the observer state X ∈ SE 2 (3) and auxiliary state Z ∈ SIM 2 (3) to have dynamics given by (15).Then define Let the initial condition of the auxiliary rotation R Z (0) = I 3 , and choose gains k p , k c > 0, K q ∈ S + (2), and k v , k d , k m ≥ 0. Define the correction terms ∆ ∈ se 2 (3) and Γ ∈ sim 2 (3) to be Denote the error state Ē ∈ SE 2 (3) as defined in (16).Assume that the vectors k m y m are persistently exciting (1).Then 1) The translational error V Ē → 0 globally exponentially.
2) The rotational error R Ē → I 3 almost-globally asymptotically and locally exponentially, with the only stable equilibrium being Ē = I 5 , and the set unstable equilibria given by 3) If the error Ē converges to I 5 , then the estimated state X converges to the true state X in the sense that | X − X| → 0. Proof: Before proving the individual items, observe that the choice R Z (0) = I 3 and Ω Γ ≡ 0 mean that R Z ≡ I 3 for all time.
Proof of Item 1: Recall from Lemma 4.3 that Thus, the correction terms are simply the sum of correction terms constructed according to Lemma 5.3.Note that S Γ includes only one instance K q , which is simply the sum of the term obtained from the individual corrections relating to position, velocity and magnetometer.Let Then, following the same computation as in the proof of Lemma 5.3 yields We proceed to show that λ( This is the continuous differential Riccati equation associated with the state dynamics and measurement matrices, which is clearly observable for any delay δ, even if k v = 0. Therefore the eigenvalues of P = A Z A ⊤ Z are bounded above and below, and L V = |V Ē | is exponentially decreasing to zero with its exponent lower-bounded by λ min (K q )λ min (P ) > 0.
Proof of Item 2: We apply Theorem 5.2.Since the proposed correction terms are the sum of corrections terms drawn from Lemma 5.3, the Lyapunov function L satisfies Therefore, each of the three individual terms must also converge to zero.By persistence of excitation [19], R Ē converges to either I 3 or to the set of rotation matrices To see that the first case is locally exponentially stable, linearise R Ē ≈ I 3 + ε× R and differentiate to obtain which is persistently exciting by assumption, and symmetric negative semi-definite.Uniform local exponential stability follows from [22,Theorem 1].
In the case that R Ē → R u , or equivalently Ē → E u , we have left to show that any such equilibrium is unstable.It suffices to show that, if is the rotational component of the Lyapunov function (19).To this end, fix R Ē ∈ R u , then R Ē has an eigenvalue equal to 1 associated with a unit vector ω ∈ R 3 ; i.e.R Ē ω = ω, |ω| = 1.Define Q(s) = R Ē exp(sω × ).Using a second-order Taylor expansion, one has Clearly, then, in any neighbourhood of R Ē ∈ R u one can find s sufficiently small so that L R (Q(s)) < L R (R Ē ).This shows that any equilibrium point Ē ∈ E u is indeed unstable.Proof of Item 3: Since Z is bounded above and below as shown in the proof of item 1, the operation E → ZEZ −1 is bounded also.Therefore, by [16,Lemma 5.3], Ē → I 5 if and only if X X−1 → I 5 .Then, assuming boundedness of X, X → X as required.This completes the proof.

VI. SIMULATION RESULTS
The proposed observer was verified using a simulation of a flying vehicle equipped with a magnetometer and timedelayed GNSS flying in a circular trajectory of radius 50 m at a speed of 25 m/s.The true initial conditions X = (R, (v, p)) ∈ SE 2 (3) and inputs Ω, a ∈ R 3 were set to where g = 9.81e 3 ∈ R 3 .The magnetic reference and the GNSS delay were defined by ẙm = e 1 , δ = 0.2.
The observer proposed in Theorem 5.2 was implemented with the following conditions.The estimated state X = ( R, (v, p)) ∈ SE 2 (3) was initialised with an extreme initial attitude error, and the auxiliary state Z ∈ SIM 2 (3) was initialised by The gains were chosen to be Both the system and observer equations were implemented using Lie group Euler integration at 50 Hz for 20 s.Additionally, a second copy of the observer was implemented without correction for the GNSS delay; that is, the observer dynamics were implemented with δ = 0 although the measurements received were still delayed.The first observer requires the delay matrices described in Lemma 4.1 to implement the correction terms.Since the right-delay matrix Y δ R (t) cannot be constructed without access to the input U (t−δ), this matrix was initialised at the time t = δ.For the period t ∈ [0, δ), the correction terms ∆ and Γ were both set to zero.Thanks to the synchrony of the error, this meant Ė = 0 and hence L ( Ē) is constant during this period.
Figure 1 shows the estimated and true states over time.Figure 2 shows the estimation errors and the Lyapunov function value.The estimates from the delay-compensated observer are shown to quickly converge to the true values, and this is reflected in the exponentially decreasing Lyapunov value.In contrast, the second observer (without delay compensation) is shown to converge more slowly during the transient phase, and fails to converge fully in the steady state with errors of approximately 3.5 deg in attitude, 2.5 m/s in velocity, and 5 m in position.This failure to converge is also seen in the Lyapunov value, which quickly plateaus.These results not only verify the proposed observer design, but also demonstrate the importance of delay compensation to ensure accurate estimation in INS.

VII. CONCLUSION
This paper studied the problem of INS with magnetometer and delayed GNSS measurements by extending recent work on synchronous observer design for INS [15].Delay matrices that are recursively defined through differential equations  are shown to relate the delayed GNSS measurements to the present state of the system.Using this relationship, correction terms were proposed that yield almost-globally asymptotic and locally exponential stability of the observer error dynamic.Finally, the simulation results showed that the proposed observer design is able to converge from extreme initial error, and that the compensation of time-delay contributes to both transient and steady-state performance.
LVSince L is the composition sum and product of uniformly continuous signals, it itself is uniformly continuous.Thus, as the cost L ( Ē) is bounded above by its initial value and below by zero, and V Ē → 0 by item 1, applying Barbalat's lemma [21, Lemma 4.2/4.3]yields