Spline-Based Factor-Graph Optimization with High-Grade Inertial Sensors

Kyle Leland; Clark Taylor; David Woodburn,; Randal Beard

doi:10.33012/navi.742

Abstract

Inertial measurement units (IMUs) are central to global navigation satellite system-based and alternative navigation solutions. This paper combines three lines of research to explore a novel methodology for using inertial sensors: factor graphs, spline-based trajectory estimation, and high-grade inertial sensing. Spline-based factor-graph trajectory estimation is increasingly used in the literature, especially for asynchronous or high-rate sensors. However, prior models neglect the impact of the Earth’s rotation, which is significant for high-grade IMUs. We extend spline-based factor graphs to incorporate accelerometer and gyroscope models that account for the Earth’s rotation. We apply this approach to simulated data from high-grade inertial sensors for two evaluations. First, we evaluate the estimation accuracy using the proposed approach and demonstrate a consistent one-order-of-magnitude improvement after 60 s of inertial coasting when compared with the literature’s spline-based approaches with coarse inertial measurement models. Then, we compare two spline-based trajectory representations applied to high-grade inertial navigation.

Keywords

1 INTRODUCTION

Precise and reliable platform localization is a core component for autonomous and unmanned navigation. Inertial sensors commonly form the backbone of precise trajectory estimation in both global navigation satellite system (GNSS)-based and alternative navigation solutions. This paper combines three unique lines of research to explore a novel methodology for using inertial sensors: factor graphs, spline-based trajectory estimation, and high-quality inertial sensing. First, factor graphs have received increased interest as a framework for fusing data from multiple sensors for maximum a posteriori (MAP) state estimation (Taylor & Gross, 2024). While factor graphs have demonstrated the ability to handle complex and highly nonlinear systems, they traditionally model a set of discrete states, at least one state for every measurement sample time. This approach has drawbacks when asynchronous sensors, such as neuromorphic cameras, rolling-shutter cameras, or simply any fusion of sensors without aligned sample times, are incorporated into a navigation solution. Additionally, this discrete-time trajectory estimation poses challenges with high-rate sensors, where an abundance of measurements can lead to an explosion in the number of states to be estimated and, therefore, an explosion in computational complexity. To overcome these problems, prior work has applied factor graphs to estimate a continuous-time trajectory rather than a trajectory at a set of discrete times (Anderson et al., 2015; Furgale et al., 2012). This approach has become increasingly popular in the literature for incorporating various sensors that either are difficult to synchronize or provide measurements at a high rate (Mueggler et al., 2015; Ng et al., 2021; Oth et al., 2013).

One method for estimating a continuous-time trajectory is to use B-splines, where a continuous trajectory is represented by a finite set of control points (first proposed by Furgale et al. (2012)). While using splines to represent translational movement (i.e., movement in ℝ³) is relatively straightforward, representing a trajectory that also moves through rotational space (SO(3)) is more difficult. To solve this problem, Patron-Perez et al. (2015) adapted cumulative B-splines to apply them to Lie groups, such as SO(3) (rotation) or SE(3) (rotation and translation). Several works in the literature have applied B-splines over Lie groups in order to represent a full trajectory and to enable the incorporation of inertial measurements from accelerometers and gyroscopes (Hug et al., 2022; Sommer et al., 2020; Sommer et al., 2016). However, these published approaches for incorporating inertial measurements into a spline-based factor-graph framework employ inertial sensor models that do not account for the effects of the Earth’s rotation on inertial measurements. High-quality inertial measurement units (IMUs), such as tactical-grade and navigation-grade IMUs, have sufficient precision for these effects to significantly impact the state estimate (Tang et al., 2022). As a result, when a high-grade IMU is available, which is commonly the case in airborne applications, the published inertial measurement models for spline-based estimation fail to fully exploit the IMU’s precision, limiting the achievable accuracy of the navigation solution. This paper presents an approach for incorporating refined inertial measurement models into a spline-based factor graph, thereby accounting for the effects of the Earth’s rotation on accelerometer and gyroscope measurements. This approach is then applied to simulated data from high-grade inertial sensors to demonstrate the improvement of the refined sensor models. Next, two potential trajectory representations are compared: a single B-spline in SE(3) and a split representation composed of B-splines in SO(3) and ℝ³. With simulated inertial measurements, the results of using each trajectory representation for navigation via factor-graph optimization are evaluated based on the accuracy of the position estimates. The intent of these evaluations is to explore effective methods for incorporating raw inertial measurements from high-grade sensors into a multi-sensor, spline-based factor-graph navigation solution.

To provide a clear description of the development and evaluation of the proposed approach, the remainder of the paper is organized as follows. Section 2 provides a brief review of factor-graph optimization and B-spline trajectory estimation. Section 3 presents the proposed approach for incorporating a refined inertial sensor model into spline-based factor-graph optimization. Section 4 conducts two comparative analyses using simulated data sets. First, the benefits of the refined inertial models are demonstrated by comparing the proposed implementation against an implementation using inertial models in the literature for spline-based factor graphs. Second, the unified-spline (SE(3)) and split-spline (SO(3) × ℝ³) trajectory representations are compared in the context of navigation with high-grade inertial sensors via a Monte Carlo evaluation with simulated measurements along randomly generated paths. Finally, Section 5 provides relevant conclusions and potential avenues of further research.

2 TECHNICAL BACKGROUND

To properly introduce our formulation of high-quality inertial sensors with a continuous-time factor graph, several topics must be clearly defined. In this section, we will review and introduce our notation for factor graphs (including continuous-time factor graphs) and cubic B-splines for Lie groups. This discussion will lay the foundation for Section 3, which provides the approach for incorporating improved measurement models for inertial sensors into a spline-based factor-graph formulation.

2.1 Factor Graphs

Factor graphs are a mathematical framework, proposed by Kschischang et al. (2001), that models the posterior distribution of state estimates based on observations. This framework opens the door for applying various elements from graph theory, linear algebra, and nonlinear optimization research to develop methods for accurate, robust, and efficient state estimation.

2.1.1 Bipartite Graph Representation

This state estimation problem may be represented by a bipartite graph with two types of vertices: states and factors. Each factor vertex encapsulates stochastic information and is connected by edges to the relevant state or states. Commonly, a factor vertex is a probabilistic model of a noisy sensor measurement given the relevant states, and it is connected by edges to those states. Factor graphs may perform estimations over batches of time, where each time step will have state vertices connected to the factor vertices related to the states at that time. Figure 1 depicts an example of a simple factor graph formulated in this manner, with states at multiple time steps, factors potentially corresponding to sensor measurements at those times, and dynamics factors modeling the relationship between states at successive time steps.

FIGURE 1

A basic factor-graph depiction where state estimates x_n at time n are informed by probabilistic models of sensor measurements at time n (represented by black squares) and by probabilistic models of the dynamics between successive states (represented by green squares)

2.1.2 Weighted Least-Squares Optimization

To solve a factor graph, the goal is to find a set of estimates $\hat{X}$ for the set of states X with the MAP probability, given a set of observations Z:

$\begin{matrix} \hat{X} = \arg \max p (X | Z) \\ X \end{matrix}$ 1

By applying Bayes’ theorem, assuming conditional independence of the observations z_i in Z, and simplifying, we obtain a form of Equation (1) that is equivalent to the following maximization problem:

$\hat{X} = \underset{X}{\arg \max} \prod_{z_{i} \in Z} p (z_{i} ∣ X)$ 2

By modeling each observation as a function of the states with additive white Gaussian noise, $z_{i} = h_{i} (X) + η_{i}, η_{i} \sim N (0, \sum_{i})$ , Equation (2) becomes a nonlinear weighted least-squares (WLS) optimization problem:

$\hat{X} = \underset{X}{\arg \min} {\sum_{z_{i} \in Z} ‖ Z_{i} - h_{i} (X) ‖}_{\sum_{i}}^{2}$ 3

The operator ${‖ \cdot ‖}_{\sum}$ denotes the Mahalanobis distance, such that ${‖ ν ‖}_{\sum} = \sqrt{ν^{⊤} \sum^{- 1} ν}$ . In factor-graph optimization, each factor is defined by its cost function $e_{i} = {‖ z_{i} - h_{i} (X) ‖}_{\sum_{i}}$ and the Jacobian matrix of the cost function with respect to the related states. As demonstrated by Dellaert and Kaess (2006) the equivalent matrix representation of the nonlinear WLS problem is typically sparse and can therefore be efficiently solved by iteratively applying linear algebra algorithms, such as QR or Cholesky decomposition, in a nonlinear least-squares optimization procedure, such as the Gauss-Newton (Dennis & Schnabel, 1996) or Levenberg-Marquardt (Marquardt, 1963) algorithms.

2.2 Continuous-Time Trajectory with B-Splines

While factor graphs for trajectory estimation were originally applied for estimating states at discrete times, further research developed approaches for estimating a trajectory in continuous time by representing the trajectory with B-splines (Furgale et al., 2012). A cubic B-spline is C²-continuous, resulting in a continuous position, velocity, and acceleration. This feature is a desirable quality for optimization with acceleration information, such as would arise from an accelerometer. As an example, a one-dimensional cubic B-spline is shown in Figure 2, where the x-axis represents time and the y-axis represents the position of the spline over time. To utilize a spline for pose estimation, we first introduce a formulation of a cubic B-spline in ℝ³, which could represent the three-dimensional position. Then, we introduce a formulation for a cubic B-spline over a Lie group, as first demonstrated by Patron-Perez et al. (2015). In a trajectory representation, Lie groups may be applied to represent rotation (SO(3)) or a combination of rotation and translation (SE(3)).¹ The following discussions of cubic B-splines in ℝ³ and in a Lie group also include a brief description of how these splines are estimated within a factor-graph framework.

FIGURE 2

A simple cubic B-spline in ℝ

The position at any time along the trajectory is dictated by four control points: the two preceding (or equal to) the current time and the two after the current time. A cubic B-spline is C²-continuous.

2.2.1 Cubic B-Spline

A cubic B-spline can represent the position in continuous time through the following formulation (Cox, 1972; de Boor, 1978). With n + 2 evenly spaced spline knot points at times [t_–1, t₀, …, t_n+1] and a continuous time t that falls in the interval t_i ≤ t < t_i+1, the value of u is u(t) = (t – t_i)/∆t, where ∆t is the uniform time difference between each knot point. The position on the spline at time t ∈ [t_i, t_i+1) is then dictated by the control points {p_i–1, p_i, p_i+1, p_i+2} through the following expression:

$\begin{matrix} p (u) = p_{i - 1} + \sum_{j = 1}^{3} {\tilde{B}}_{j} (u) (p_{i + j - 1} - p_{i + j - 2}), p_{i} \in ℝ^{3} \\ \tilde{B} (u) = C [\begin{matrix} 1 & u & u^{2} & u^{3}]^{⊤}, C = \frac{1}{6} [\begin{matrix} 6 & 0 & 0 & 0 \\ 5 & 3 & - 3 & 1 \\ 1 & 3 & 3 & - 2 \\ 0 & 0 & 0 & 1 \end{matrix}] \end{matrix} \end{matrix}$ 4

where the subscript j of ${\tilde{B}}_{j}$ indicates the j-th element of $\tilde{B}$ (zero-based). The coefficients ${\tilde{B}}_{j}$ are cumulative spline coefficients, as they provide an aggregated expression of the general B-spline coefficients that is specific to a cubic spline (Qin, 1998). This B-spline formulation can be applied within a factor graph by including the control points ${p_{- 1}, p_{0}, \cdot \cdot \cdot, p_{n + 1}}$ as states in the factor graph, where n is the number of intervals defined on the spline. If the position is represented by a cubic B-spline, then the estimated position at any time on the spline may be sampled using Equation (4) and the MAP estimate of the control point states. Typically, the factors within the graph do not provide information directly related to the control points. For example, a direct position measurement provides information related to the position estimate at a specific time, and that position estimate is a function of the estimated control points. Thus, optimizing a spline-based factor graph requires the Jacobian matrices of the factors’ cost functions with respect to the control points. This additional layer of abstraction requires the application of the chain rule from basic calculus, as the error is typically a function of the position estimate at a specific time and that position estimate is a function of four control points. Then, through a nonlinear least-squares optimization procedure, the control point states may be updated to reach a MAP estimate, which then forms a continuous-time estimate of the position throughout the spline’s defined intervals.

2.2.2 Cubic B-Spline in a Lie Group

While Equation (4) can represent translation in ℝ³, many applications require estimating the full pose of a platform, which would also include rotation. Lie groups are a popular mathematical tool in these applications, including the Lie groups SO(3), which can represent rotations, and SE(3), which can represent the combination of translations and rotations. Patron-Perez et al. (2015) adapted the cumulative, cubic B-spline formulation in Equation (4) to form a B-spline over Lie groups by replacing the arithmetic addition and subtraction operations in Equation (4) with matrix exponential and natural logarithm operations, respectively.

To provide the mathematical background for the spline-based pose estimation in this paper, we first consider a formulation for a B-spline in SO(3). The attitude of a platform may be represented by a B-spline over the rotation matrices in SO(3), where R_w,t ∈ SO(3) is the rotation matrix from the platform’s body frame to the world frame at time t. The spline is defined by control points R_w,i ∈ SO(3). The attitude at time t where t is in [t_i, t_t+1) is dictated by the control points {R_w,i–1, R_w,i, R_w,i+1, R_w,i+2} through the following expression:

$\begin{matrix} R_{w, t} (u) = R_{w, i - 1} \exp ({\tilde{B}}_{1} (u) Ω_{i - 1, i}) \exp ({\tilde{B}}_{2} (u) Ω_{i, i + 1}) \exp ({\tilde{B}}_{3} (u) Ω_{i + 1, i + 2}) \\ Ω_{j, k} = l o g (R_{w, j}^{- 1} R_{w, k}) \end{matrix}$ 5

If factors provide information not directly related to the attitude, but to the angular velocity or angular acceleration, then the expressions for $\frac{\partial R_{w, t}}{\partial t} = {\dot{R}}_{w, t}$ and $\frac{\partial^{2} R_{w, t}}{\partial t^{2}} = {\ddot{R}}_{w, t}$ , respectively, are required to form the factor graph. By applying the product rule from basic calculus to Equation (4), we obtain the first and second derivatives of the attitude on the spline with respect to time as follows:

${\dot{R}}_{w, t} (u) = R_{w, i - 1} ({\dot{A}}_{0} A_{1} A_{2} + A_{0} {\dot{A}}_{1} A_{2} + A_{0} A_{1} {\dot{A}}_{2})$ 6

$\begin{matrix} {\ddot{R}}_{w, t} (u) = R_{w, i - 1} ({\ddot{A}}_{0} A_{1} A_{2} + A_{0} {\ddot{A}}_{1} A_{2} + A_{0} A_{1} {\ddot{A}}_{2} + 2 {\dot{A}}_{0} {\dot{A}}_{1} A_{2} + 2 {\dot{A}}_{0} A_{1} {\dot{A}}_{2} + 2 A_{0} {\dot{A}}_{1} {\dot{A}}_{2}) \\ A_{j} = \exp ({\tilde{B}}_{j} (u) Ω_{i, j}), {\dot{A}}_{j} = {\dot{\tilde{B}}}_{j} (u) A_{j} Ω_{i, j}, {\ddot{A}}_{j} = {\ddot{\tilde{B}}}_{j} (u) A_{j} Ω_{i, j} + {\dot{\tilde{B}}}_{j} (u) {\dot{A}}_{j} Ω_{i, j} \\ \dot{\tilde{B}} (u) = \frac{1}{Δ t} C [\begin{matrix} 0 & 1 & 2 u & 3 u^{2}], \ddot{\tilde{B}} (u) = \frac{1}{Δ t^{2}} C [\begin{matrix} 0 & 0 & 2 & 6 u] \end{matrix} \end{matrix} \end{matrix}$ 7

The spline formulation in Equation (5) and the time derivatives in Equations (6) and (7) may also be applied to form a spline in SE(3) that represents both translation and rotation. This step is done by simply replacing the control points ${R_{w, - 1}, R_{w, 0}, \cdot \cdot \cdot, R_{w, n + 1}}$ in SO(3) with pose matrices in SE(3). The closed-form expressions of the exponential and natural logarithm operations for SO(3) as well as for SE(3) have been provided by Blanco (2010). As described before, optimizing a factor graph with a spline-based trajectory representation includes iterative updates to the control points, rather than direct updates of the pose estimates at specific time points. In the implementation presented in this paper, the SO(3) control points are updated using a vector from the corresponding Lie algebra $𝔰 𝔬 (3)$ , as described by Blanco (2010). Each control point R_w,i with an associated update vector $ϵ_{i} \in 𝔰 𝔬 (3)$ is updated such that $R_{w, i} \leftarrow R_{w, i} Exp (ϵ_{i})$ , where Exp(‧) is the exponential map from $𝔰 𝔬 (3)$ to SO(3) defined by Blanco (2010). States in SE(3) are similarly updated with a vector in the Lie algebra $𝔰 𝔢 (3)$ , such that $T_{w, i} \leftarrow T_{w, i} Exp (ϵ_{i})$ . Optimizing the factor graph then requires the Jacobian matrices of an attitude (or pose) on the spline with respect to exponential updates applied to each of the Lie group control points. The necessary Jacobian matrices are derived in Appendix A. With these Jacobian matrices and the application of the chain rule for derivatives, the cost function in Equation (3) can be linearized about the current state estimate, and various linear algebra approaches may be applied to iteratively solve the resulting linear least-squares problems to update the state and reduce the error norm.

2.3 Inertial Measurements in a Factor Graph

This paper is specifically focused on an improved method for incorporating raw inertial measurements in a spline-based factor graph. To give context to this work and the proposed approach, we first provide a brief summary of related work using inertial measurements in factor graphs, including works using traditional discrete-time trajectory representations. Incorporating inertial measurements into a factor graph requires a probabilistic model for the expected sensor measurements given the estimated states. A straightforward approach is to estimate the relevant states (e.g., position, velocity, and attitude) at the discrete time when the measurement was sampled. However, inertial sensors typically have a high sampling rate, commonly on the order of 100 Hz. If the state were estimated at the discrete sample time of each measurement, then representing the state at the discrete time of each measurement would result in a large number of states to be estimated. To reduce the computational cost of including inertial measurements in a factor graph, Forster et al. (2017) proposed a technique that integrates batches of accelerometer and gyroscope measurements and forms a single, pre-integrated factor from a batch of measurements from a sensor. Tang et al. (2022) refined the pre-integration approach by incorporating the effects of the Earth’s rotation on the expected accelerometer and gyroscope measurements with significant improvement in the estimated position and attitude. Although this approach substantially reduces the computational complexity of incorporating IMU measurements, the pre-integrated inertial factors are still linked to a discrete-time representation of the state, which presents a challenge when incorporating additional, asynchronous sensors.² This challenge is alleviated by continuous-time estimation with spline-based factor graphs, which have been successfully applied for combining inertial measurements with measurements from event-based cameras (Mueggler et al., 2015), lidar (Lv et al., 2021), and rolling-shutter cameras (Patron-Perez et al., 2015). However, spline-based factor formulations in the literature that incorporate inertial measurements use measurement models that do not account for the Earth’s rotation:

$h_{a} (t) = R_{w, t}^{⊤} ({\ddot{p}}_{w} - g_{w}) + b_{a}$ 8

$h_{g} (t) = {(R_{w, t}^{⊤} {\dot{R}}_{w, t})}^{\lor} + b_{g}$ 9

Here, for the accelerometer model, h_a (t) is the expected accelerometer measurement, R_w,t is the attitude from the SO(3) spline, ${\ddot{p}}_{w}$ is the second derivative of the position in the world frame, g_w is the gravity vector in the world frame, and b_a is the estimated bias on the accelerometer, all at time t. For the gyroscope model, h_g (t) is the expected gyroscope measurement, ${\dot{R}}_{w, t}$ is the first derivative of the attitude matrix, and b_g is the estimated bias of the gyroscope, all at time t. The “vee” operator ^v is the operation that converts a skew-symmetric matrix to the corresponding vector in ℝ³, as described by Blanco (2010). This coarse inertial measurement model neglects the rotation of the Earth and is used in the literature for spline-based factor-graph approaches (Hug et al., 2022; Lv et al., 2021; Ng et al., 2021; Patron-Perez et al., 2015).

3 SPLINE-BASED FACTOR GRAPH FOR HIGH-GRADE INERTIAL SENSORS

This paper develops an approach for including a high-precision inertial model in a spline-based factor graph. Building on previously published approaches, the model presented in this paper refines the precision by introducing terms to the accelerometer and gyroscope measurement models that account for the effects of the Earth’s rotation. The proposed inertial measurement and bias factors are incorporated into a factor graph, along with supplementary prior factors and altitude measurement factors for proper initialization and stability. Each of these factors are defined, and then, two potential spline-based trajectory representations are discussed. Figure 3 provides a graphical depiction of the proposed implementation.

FIGURE 3

In this depiction of the proposed approach, the circles with x_i represent control point and sensor bias state estimates at spline knot time i, and the blocks represent factors, including a prior factor (magenta), inertial measurement factors (cyan), and bias transition factors (yellow).

3.1 High-Precision Inertial Factors

The accelerometer and gyroscope measurement models are as follows:

$h_{a} (t) = {(R_{e, w} R_{w, t})}^{⊤} (R_{e, w} {\ddot{p}}_{w} + 2 ω_{e, e i} \times (R_{e, w} {\dot{p}}_{w}) - g_{e}) + b_{a}$ 10

$h_{g} (t) = {({(R_{e, w} R_{w, t})}^{⊤} (R_{e, w} {\dot{R}}_{w, t}))}^{\lor} + {(R_{e, w} R_{w, t})}^{⊤} ω_{e, e i} + b_{g}$ 11

where R_w,t is the estimated attitude represented as a rotation matrix from the body frame (at time t) to the local w-frame, p_w is the estimated position in the w-frame dictated by the position spline control points, R_e,w is the rotation matrix from the w-frame to the Earth-centered, Earth-fixed (ECEF) frame, ω_e,ei is the rotation rate of the ECEF frame relative to the inertial frame (represented in the ECEF frame), and g_e is the expected gravity vector in the ECEF frame, given the position estimate. The terms b_a and b_g represent the accelerometer and gyroscope bias terms, respectively. The term $2 ω_{e, e i} \times (R_{e, w} {\dot{p}}_{w})$ in Equation (10) models the Coriolis acceleration due to the rotation of the Earth, and the term ${(R_{e, w} R_{w, t})}^{⊤} ω_{e, e i}$ in Equation (11) models the rotation of the w-frame relative to the inertial frame. By applying the properties of SO(3) matrices such that $R_{b, a}^{⊤} R_{b, a} = I_{3}$ as well as $R_{a, b} (v \times w) = R_{a, b} v \times R_{a, b} w$ ,we can simplify Equations (10) and (11) as follows:

$h_{a} (t) = R_{w, t}^{⊤} ({\ddot{p}}_{w} + 2 ω_{w, e i} \times {\dot{p}}_{w} - g_{w}) + b_{a}$ 12

$h_{g} (t) = {(R_{w, t}^{⊤} {\dot{R}}_{w, t})}^{\lor} + R_{w, t}^{⊤} ω_{w, e i} + b_{g}$ 13

where $ω_{w, e i} = R_{e, w}^{⊤} ω_{e, e i}$ is the rotation of the ECEF frame relative to the inertial frame represented in the local w-frame and g_w is the modeled gravity vector at the current position estimate represented in the w-frame. Incorporating these measurement models into a factor-graph optimization framework also requires the Jacobian matrices of h_a (t) and h_g (t) relative to changes in the state estimates, specifically updates applied to the relevant control points and bias estimates for time t. These Jacobian matrices are a key contribution of this paper; for readability, they are derived and provided in Appendix B for Equation (12) and in Appendix C for Equation (13). The costs associated with each accelerometer and gyroscope measurement are then as follows:

$e_{a} = {‖ z_{a, t} - h_{a} (t) ‖}_{\sum_{a}}$ 14

$e_{g} = {‖ z_{g, t} - h_{g} (t) ‖}_{\sum_{g}}$ 15

where z_a,t is the accelerometer measurement and z_g,t is the gyroscope measurement at time t. The covariance matrices Σ_a and Σ_g are the discretized accelerometer and gyroscope sensor noise covariance matrices, respectively. The noise on the bias states, b_a and b_g, is modeled as first-order Gauss–Markov noise such that, for sensor s, we have the following:

$b_{s, t + Δ t} = b_{s, t} \exp (- \frac{Δ t}{τ_{s}}) + η_{b, s}, η_{b, s} \sim N (0_{3}, \sum_{b, s})$ 16

where b_s,t is the bias of sensor s at time t, τ_s is the time correlation constant associated with the sensor’s bias, and Σ_b,s is the discretized bias instability noise covariance of the sensor, as thoroughly described by Brody et al. (2023). This bias noise model results in the corresponding cost function for two successive estimated bias states:

$e_{b, s} = {‖ b_{s, t_{2}} - b_{s, t_{1}} \exp (- \frac{t_{2} - t_{1}}{τ_{s}}) ‖}_{\sum_{b, s}}$ 17

This cost function, along with its Jacobian matrices, forms a bias-transition factor that relates successive bias estimates of the accelerometer or gyroscope in the factor graph.

3.2 Supplementary Factors

In addition to the refined inertial measurement factors, this paper also includes prior factors and an altimeter factor in the factor-graph implementation. The prior factors are necessary for proper initialization of the inertial-based coasting, and the altimeter factor is included to account for the instability of the vertical channel in inertial navigation (Titterton & Weston, 2004). The implementation includes prior factors for the initial position, velocity, and attitude with the following associated cost functions:

$e_{p} = {‖ p_{w, prior} - p_{w, 0} ‖}_{\sum_{p}}$ 18

$e_{ν} = {‖ {\dot{p}}_{w, prior} - {\dot{p}}_{w, 0} ‖}_{\sum_{v}}$ 19

$e_{R} = {‖ \log {(R_{w, prior}^{⊤} R_{w, 0})}^{\lor} ‖}_{\sum_{R}}$ 20

where p_{w, prior} is the a priori expectation of the position, p_w,0 is the estimated position in the w-frame, ${\dot{p}}_{w, prior}$ is the a priori expectation of the velocity, ${\dot{p}}_{w, 0}$ is the estimated velocity vector, R_{w, prior} is the a priori expectation of the attitude represented as an SO(3) matrix, and R_w,0 is the estimated attitude as a rotation matrix, all at time t = 0. The covariance matrices $\sum_{p}, \sum_{v}$ , and Σ_R are the covariances of the prior position, velocity, and attitude, respectively. For the altimeter factors, the associated cost is as follows:

$e_{h} = {‖ z_{h, t} - p_{w, t, z} ‖}_{σ_{h}^{2}}$ 21

where z_h,t is the measured altitude converted to a displacement from the origin of the local w-frame and p_w,t,z is the third component of the position vector from the estimated spline, both at time t when the measurement was sampled. $σ_{h}^{2}$ is the scalar variance of the altimeter measurements.

3.3 State Representation

This work compares two potential spline-based trajectory representations: (1) a split representation composed of cubic B-splines in SO(3) and ℝ³ and (2) a unified cubic B-spline in SE(3). For the split-spline representation, the position is represented by a cubic B-spline in ℝ³ following the formulation in Equation (4), where the position sampled at time t on the spline is the position of the platform in the local tangent w-frame, p_w,t.The attitude is represented by a cubic B-spline in SO(3) following the formulation in Equation (5), where the attitude sampled at time t on the spline is the rotation R_w,t from the platform’s body frame to the w-frame at time t. In the case of the unified spline, the control points in Equation (5) are matrices T_w,i ∈ SE(3), rather than rotation matrices in SO(3). The result of sampling from the spline at time t is then the transformation matrix from the platform’s body frame to the w-frame at time t, T_w,t. A matrix T_w,t in SE(3) is composed of the attitude R_w,t and position p_w,t as follows:

$T_{w, t} = [\begin{matrix} R_{w, t} & p_{w, t} \\ 0_{3}^{⊤} & 1 \end{matrix}]$ 22

For either trajectory representation, the splines are formed with a sufficient number of evenly spaced control points to define the trajectory across the range of measurement times, defining n intervals with n + 3 control points. For each of the n spline intervals, there is also an accelerometer bias state b_a as well as a gyroscope bias state b_g. These sensor biases are assumed to be constant over the interval and are not smoothed by a spline; this assumption is safer (though still not perfectly valid) in the case of high-grade IMUs, which have low bias instability relative to a knot interval.

4 SIMULATED EVALUATION OF THE PROPOSED FACTOR GRAPH

This section evaluates the approach proposed in Section 3 using simulated data sets. This evaluation is divided into two comparisons. First, the proposed approach with the refined inertial measurement models in Equations (12) and (13) is compared against an approach using the coarse inertial models in Equations (8) and (9), which are found in the literature for spline-based factor graphs. Second, two potential trajectory representations—a split spline in SO(3) × ℝ³ and a unified spline in SE(3)—are compared for use in the proposed approach with the refined measurement models. These evaluations are designed to explore the incorporation of high-grade inertial sensors in a spline-based factor graph. While these evaluations are performed in the context of inertial coasting, the intent is to demonstrate an effective approach for incorporating high-grade inertial sensors into a multi-sensor solution, especially for applications that require periods of inertial coasting.

4.1 Comparison of Precise and Coarse Inertial Models

The factor-graph implementation described in Section 3 was applied to simulated measurements from three IMUs along two paths. For comparison, the same factor-graph implementation, except with the coarse measurement models in Equations (8) and (9), was also applied to each IMU and path. For both the precise and the coarse measurement model implementations, the trajectory was represented by a split spline in SO(3) × ℝ³.

4.1.1 Simulated Data Set

To evaluate the benefit of including the effects of the Earth’s rotation in the inertial measurement models in a spline-based factor graph, we generated a data set including simulated measurements from three different grades of IMUs along two paths: a realistic, 4-min figure-eight loop (Figure 4) and a 1-min randomly generated path (Figure 5).

FIGURE 4

The figure-eight path is represented in an NED tangent frame, designated the w-frame, centered at 39.76˚ N, 84.19˚, and an altitude of 226 m HAE. (a) True p_w over time, (b) True ṗ_w over time, (c) True roll, pitch, and yaw over time.

FIGURE 5

The randomly generated path is represented in an NED tangent frame, designated the w-frame, centered at 52.477˚ S, 6.595˚ W, and an altitude of 920.54 m HAE. (a) True p_w over time, (b) True ṗ_w over time, (c) True roll, pitch, and yaw over time.

This paper evaluates the proposed approach using simulated measurements from an industrial-grade, a tactical-grade, and a navigation-grade IMU, where “industrial” is the least precise and “navigation” is the most precise of these three grades. This evaluation was accomplished by first generating ideal, noiseless accelerometer and gyroscope measurements along the known, true paths using the Python library “inu” (Woodburn, 2023b). Then, we generated the simulated measurements for each sensor by adding noise to the ideal measurements in accord with the sensor’s noise parameters. Random walk noise was modeled as white noise on the individual accelerometer and gyroscope measurements, and bias instability noise was modeled as a first-order Gauss-Markov process. The noise n_s,k for a sensor s added to the ideal measurement at sample k is as follows:

$n_{s, k} = n_{r w, k} + n_{b, k}$ 23

$n_{r w, k} = ν_{k} : ν_{k} \sim N (0, σ_{r w, s}^{2} / T)$ 24

$n_{b, k} = {\begin{matrix} η_{0} & k = 0 \\ e^{- \frac{T}{τ_{s}}} η_{k - 1} + η_{k} & k > 0 \end{matrix} : η_{k} \sim (0, σ_{b, s}^{2} (1 - e^{- \frac{2 T}{τ_{s}}}))$ 25

Here, Equations (24) and (25) model the random walk noise and bias instability, respectively, whereas T is the sample period for the IMU (Hiatt & Taylor, 2022; Woodburn, 2023a). The noise parameters for each IMU were drawn from the models reported by Brody et al. (2023), who derived these models from manufacturer specifications and verified their consistency with expected drift and random walk behavior in simulation. These parameters are listed in Table 1.

View this table:

TABLE 1

Accelerometer and Gyroscope Parameters

The accelerometer and gyroscope noise parameters assessed by Brody et al. (2023) were used to simulate noisy measurements from three different IMUs. These noise values are discretized and included within the factor graph in accord with the described measurement models.

With this approach, measurements from each of the three IMUs were simulated along a 4-min figure-eight path (Figure 4) and a 1-min random path (Figure 5). This random path was generated to confirm that the results along the more-realistic figure-eight path were not an artifact of that path’s particular geometry. For each path, the local w-frame was selected as a tangent, north–east–down (NED) frame at the platform’s starting latitude, longitude, and height above ellipsoid (HAE). The random path was built starting at the origin of the w-frame by generating a cubic B-spline in SE(3) with random control points. The first control point of the spline, T_w,0, was set as the identity matrix I₄. Then, each following control point was generated by iteratively applying a random exponential update vector, such that $T_{w, i + 1} = T_{w, i} Exp (ϵ_{i})$ . This approach resulted in the random path shown in Figure 5. For each path and each IMU, the spline trajectory and bias states are estimated via two factor graphs: one with the precise measurement models in Equations (12) and (13) and another with the coarse measurement models found in Equations (8) and (9), which are also the models employed in the literature for spline-based factor graphs. The resulting position, velocity, and attitude estimates are compared with the true values.

4.1.2 Comparison of Inertial Model Results

The estimated positions, velocities, and attitudes were compared against the true values along the two paths. Including the effects of the Earth’s rotation significantly impacted the accuracy of the position, velocity, and attitude estimates for the industrial-, tactical-, and navigation-grade IMUs. The estimated position and velocity errors along the figure-eight path are shown in Figure 6, and the estimated position and velocity errors along the randomly generated path are shown in Figure 7. The position and velocity errors at each time t are calculated as the L²-norm of the difference between the true and estimated position and velocity vectors, respectively, at that time.

FIGURE 6

Position (p_w) and velocity (ṗ_w) errors of the estimated pose when using the coarse and precise models of measurements from each IMU along the figure-eight path (a) p_w error, (b) ṗ_w error.

The position (velocity) errors at time t are calculated here as the L²-norm of the difference between the true position (velocity) vector and the estimated position (velocity) vector at time t.

FIGURE 7

Position (p_w) and velocity (ṗ_w) errors of the estimated pose when using the coarse and precise models of measurements from each IMU on a randomly generated path (a) p_w error, (b) ṗ_w error.

The position (velocity) errors at time t are calculated here as the L²-norm of the difference between the true position (velocity) vector and the estimated position (velocity) vector at time t.

The simulated scenario along the figure-eight path is analogous to 4 min of coasting with inertial measurements, stabilized by low-precision altimeter measurements. After 4 min of coasting along the figure-eight path, the position and velocity estimate errors obtained when using the refined measurement model outperformed coasting with a coarse measurement model by orders of magnitude, as shown in Figure 6. Applying a coarse model that does not account for the Earth’s rotation inhibited the ability of the factor-graph optimization framework to make full use of the sensors’ precision.

After 1 min of coasting along the randomly generated path, the precise measurement model resulted in submeter position error for the navigation-grade IMU, a position error on the order of tens of meters for the tactical-grade IMU, and a position error of hundreds of meters for the industrial-grade IMU. The estimated velocity errors after 1 min of coasting spanned from centimeters per second for the navigation-grade IMU to one meter per second for the industrial-grade IMU. As with the figure-eight path, the factor-graph implementation with the proposed precise measurement models significantly outperformed the factor-graph implementation with coarse measurement models in position and velocity estimation.

The final attitude errors after 4 min of coasting on the figure-eight path are shown in Table 2, and the final attitude errors after 1 min along the randomly generated path are given in Table 3. These tables provide the absolute values of the errors of the estimated attitude at the final time step. The substantial errors in attitude estimates for the coarse measurement models explain some of the dramatic position and velocity errors. A slight error in attitude can result in a misattribution of the sensed specific force of gravity, resulting in erroneous implied acceleration. This effect grows as coasting time increases, leading to the large position and velocity errors observed for the coarse measurement models.

View this table:

TABLE 2 Attitude Errors After 4 Min of Coasting Along the Figure-Eight Path with (a) Coarse and (b) Precise Inertial Measurement Models These values are the absolute value of the roll, pitch, and yaw errors of the attitude estimate at the final time in the data set.

View this table:

TABLE 3

Attitude Errors After 1 Min of Coasting Along the Randomly Generated Path with (a) Coarse and (b) Precise Inertial Measurement Models These values are the absolute value of the roll, pitch, and yaw errors of the attitude estimate at the final time in the data set.

Further analysis of the detrimental effects of the coarse measurement models depends on an understanding of the optimization mechanics. Within the nonlinear WLS problem, measurement errors are weighted according to the inverse of the corresponding measurement covariances, such that low-variance measurements have a larger weight. In the implementation with coarse measurement models, the slight error due to the neglected terms is amplified according to the expected precision of the sensor. Thus, the errors due to the excluded terms are amplified in high-grade IMUs, which would have a particularly low variance for the noise on the accelerometer and gyroscope measurements. Because each of the selected IMUs have sufficient precision to detect the neglected terms, excluding these terms results in a misalignment between the factor graph’s sensor model and the sensor measurements. This misalignment is significant enough to inhibit coasting with the coarse inertial measurement models.

4.2 Comparison of Split-Spline and Unified-Spline Trajectory Representations

When using B-splines over a Lie group to represent the trajectory, two potential options for representing the full pose are a split spline in SO(3) × ℝ³ and a unified spline in SE(3). Both representations are used in the literature (Lv et al., 2021; Patron-Perez et al., 2015; C. Sommer et al., 2020), and this analysis will compare the two representations in the context of high-grade inertial navigation. The unified-spline representation from Patron-Perez et al. (2015) may offer benefits in precision in some path geometries owing to the coupling of rotation and translation; however, this representation is expected to be computationally more expensive than the split-spline representation. While closed-form expressions are available for the exp(‧) and log(‧) operations for SE(3), they are significantly more complex than the closed-form expressions for the same operations for SO(3). In a split-spline representation, the simpler SO(3) operations are paired with the linear operations of Equation (4) for the position spline, which is expected to require less computation for optimization than the unified spline. Furthermore, for raw inertial factors, the Jacobian of the second time derivative of position (acceleration) with respect to the spline control points is required, whereas the Jacobian of the second time derivative of attitude (angular acceleration) is not required, as shown in Equations (12) and (13). In the split-spline case, this Jacobian can be obtained efficiently, as acceleration depends linearly on the position spline. In contrast, a unified spline requires differentiation of the second time derivative of the full SE(3) pose, which introduces additional computational cost due to nonlinear exponential and logarithmic operations. The concern in this comparison is whether this benefit of reduced computational complexity comes at the cost of accuracy in the context of inertial navigation with high-grade sensors. Thus, the following analysis provides a brief comparison of the two representations applied to navigation with high-grade inertial sensors. The two trajectory representations are compared based on position errors after 60 s of inertial navigation using the approach described in Section 3.

The data set for the trajectory representation comparison was comprised of 100 Monte Carlo iterations of simulated measurements on a 1-min randomly generated path for each of the three selected sensors. For each sensor on each iteration, two factor-graph implementations were separately applied to estimate the trajectory— an implementation with a split-spline representation in SO(3) × ℝ³ and an implementation with an SE(3) spline representation. Both implementations used the precise measurement models in Equations (12) and (13).

Because, for each iteration and sensor, the two trajectory representations are tested on the same simulated noisy measurements, the resulting position errors form paired samples. In this case, the paired differences were distinctly non-normal, violating an assumption of the common paired t-test. The Wilcoxon signed-rank test is a nonparametric test that can be applied to non-normal, symmetrically distributed paired differences. When applied to the paired differences of 1-min position estimate errors from the SE(3) and SO(3) × ℝ³ spline approaches at α =0.05, the Wilcoxon signed-rank test failed to reject the null hypothesis that the median difference in the position errors between the two implementations is zero. While no statistically significant median difference between the errors of the two approaches was found, the 60-s position estimate errors obtained by using an SE(3) spline had a higher maximum for each of the three sensor grades, as shown in Figure 8. The violin plots in this figure also portray the distribution of 60-s position errors from the 100 iterations for each sensor grade.

FIGURE 8

Position errors after 60 s of inertial coasting obtained from 100 Monte Carlo runs of randomly generated noise from each sensor (a) Navigation grade, (b) Tactical grade, (c) Industrial grade.

For each Monte Carlo iteration and each sensor, the trajectory is estimated via factor-graph optimization with two trajectory representations: a single spline in SE(3) and a split spline in SO(3) × ℝ³. For each trajectory representation in these violin plots, the width of the shape corresponds to the frequency of a given magnitude of position error, and the horizontal lines mark the maximum, median, and minimum of the errors.

While the split spline in SO(3) × ℝ³ and single spline in SE(3) trajectory representations were comparable in estimation accuracy for the randomly generated scenarios, the SE(3) trajectory implementation required a substantially higher computational load. For 60 s of simulated measurement data, factor-graph optimization of the SO(3) × ℝ³ implementation required an average of 3.78 s per Gauss-Newton iteration, whereas optimization of the SE(3) implementation required an average of 9.85 s per Gauss-Newton iteration. These experiments were conducted on an Intel i7-13700F central processing unit with 16 GB of random-access memory. A significant portion of the Python code was “just-in-time”-compiled using Numba; the compilation was single-threaded, and no graphics processing unit acceleration was used. Although the runtimes are suboptimal compared with optimized and fully compiled code, the relative computational load between the two approaches should be representative, as both spline-based factor-graph approaches were implemented analogously.

5 CONCLUSION

Using a simulated data set including three IMUs and two paths, this paper has demonstrated the improved position, velocity, and attitude accuracy offered by the presented inertial measurement models when compared with the coarse inertial measurement models reported in the published literature for spline-based, factor-graph implementations. Notably, the estimated positions and velocities after 60 s of coasting consistently improved by an order of magnitude after the coarse measurement factors were replaced with the precise measurement factors on both paths. The necessary components for incorporating these precise measurement models into a continuous-time, factor-graph implementation are provided in Section 3 and in further depth in the appendices. The coarse inertial measurement models employed in previously published spline-based, factor-graph implementations may be sufficient in applications that integrate low-grade IMUs with visual or GNSS sensors, which would constrain the absolute position error. In contrast, the approach presented here for high-precision inertial measurement models provides significant benefits for multi-sensor, spline-based factor-graph applications that incorporate high-grade IMUs, particularly in scenarios that include periods of inertial coasting. Additionally, an SE(3) spline trajectory representation and an SO(3) × ℝ³ split-spline trajectory representation were compared in the context of the proposed approach through Monte Carlo iterations with simulated noisy data. No significant difference in estimation accuracy was demonstrated, although optimizing the SE(3) spline implementation required significantly more time. The three primary contributions of this work are (1) high-precision sensor models of inertial measurements and the necessary components for inclusion in a continuous-time, factor-graph formulation, (2) a comparative study through a simulated data set of two paths and three IMUs to evaluate the proposed models against an approach in the published literature for including inertial sensors in a spline-based factor graph, and (3) a comparison between an SE(3) spline and an SO(3) × ℝ³ split-spline trajectory representation in the context of navigation with high-grade inertial sensors.

As the results presented in this paper are based entirely on simulated IMU measurements using the parameters verified by Brody et al. (2023), several limitations should be noted when considering real-world deployment. In practice, the performance of high-grade IMUs may degrade owing to environmental factors such as mechanical vibrations, temperature variations, magnetic interference, and other operational stresses that are not captured in idealized simulations. Additionally, real sensors may exhibit time-varying bias characteristics or other nonlinear behaviors not fully modeled here. All simulations were conducted with a fixed 100-Hz sampling rate; although higher sampling rates would not alter the mathematical framework of the proposed approach, they would increase the number of IMU factors per spline interval, resulting in design matrices with more rows in the factor-graph optimization. However, because the number of estimated states, and therefore the number of columns, remains unchanged, the primary determinant of computational cost—scaling roughly with the square of the number of columns in this context—is not affected (Trefethen & Bau, 2022). Importantly, while these real-world factors may degrade the absolute estimation performance compared with idealized simulations, the relative benefit of the proposed high-precision inertial measurement models over coarse inertial models in spline-based factor graphs should remain. Future work could validate the proposed approach on experimental IMU data sets and investigate the integration of asynchronous sensors with the proposed approach.

HOW TO CITE THIS ARTICLE:

Leland, K., Taylor, C., Woodburn, D., & Beard, R. (2026). Spline-based factor-graph optimization with high-grade inertial sensors. NAVIGATION, 73. https://doi.org/10.33012/navi.742

A | JACOBIANS OF UPDATES TO SPLINE CONTROL POINTS IN A LIE GROUP

This section provides expressions for the Jacobian matrices of the estimated pose and time derivatives of the pose with respect to incremental updates to the control points for a cubic B-spline in SO(3) with evenly spaced knots. With a vector $ϵ_{i} \in 𝔰 𝔬 (3)$ , control point R_w,i is updated via a retraction on the SO(3) manifold such that $R_{w, i} \leftarrow R_{w, i} \exp (ϵ_{i})$ . The derivative $\frac{\partial R_{w, i}}{\partial ϵ_{i, j}}$ is then a (3 × 3) matrix, where each element on the k-th row and l-th column of the matrix is the derivative of the element R_w,i,(k,l) with respect to the j-th element of the update vector є_i. The analytic expression of this matrix has been given by Blanco (2010). Finding the Jacobian matrices for the spline control points also requires the derivatives $\frac{\partial}{\partial Ω} \exp (Ω)$ and $\frac{\partial}{\partial R} \log (R)$ ; analytic, closed-form expressions of these derivatives have been provided by Blanco (2010). The Jacobian matrices with respect to updates to control points of a cubic B-spline in SE(3) may be evaluated with the same expressions presented here, although the closed-form expressions for the corresponding exponential and logarithmic operations for SE(3) would need to be substituted. These expressions have also been provided by Blanco (2010).

A.1 Prerequisite Derivatives

The expression given in Equation (5) for calculating the pose at a given time on the spline includes the skew-symmetric matrix representing the rotation between successive control points, Ω_i,j = log(R_i,wR_w,j). Thus, calculating the Jacobian matrices for updates to the control points requires the derivative of each Ω_i,j with respect to updates to both R_w,i and R_w,j. The chain rule for scalar derivatives can be generalized to matrix derivatives via Einstein notation, where, for example, the expression (M)_ab = (N)_abcd (O)_cd denotes that the entries of M are $m_{a b} = \sum_{c} \sum_{d} n_{a b c d} o_{c d}$ . In this expression, M, N, and O are tensors in which the dimensions of M are denoted by a and b, the dimensions of N are denoted by a, b, c, and d, and the dimensions of O are denoted by c and d. Now, the derivative of the matrix Ω_i,j with respect to the k-th element for applying the update vector є_j to control point R_w,j can be expressed as follows:

${(\frac{\partial Ω_{i, j}}{\partial ϵ_{j, k}})}_{a b} = {(\frac{\partial \log (R_{i, w} R_{w, j})}{\partial (R_{i, w} R_{w, j})})}_{a b c d} {(R_{i, w} \frac{\partial R_{w, j}}{\partial ϵ_{j, k}})}_{c d}$ 26

where $\frac{\partial Ω_{i, j}}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}, \frac{\partial \log (R)}{\partial (R)} \in ℝ^{(3 \times 3) \times (3 \times 3)}$ , and $\frac{\partial R_{w, j}}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}$ . Similarly, the derivative of the matrix Ω_i,j with respect to the k-th element for applying the update vector є_i to control point R_w,i can be expressed as follows:

${(\frac{\partial Ω_{i, j}}{\partial ϵ_{i, k}})}_{a b} = {(\frac{\partial \log (R_{i, w} R_{w, j})}{\partial (R_{i, w} R_{w, j})})}_{a b c d} {(({(\frac{\partial R_{i, w}}{\partial R_{w, i}})}_{c d e f} {(\frac{\partial R_{w, i}}{\partial ϵ_{i, k}})}_{e f}) R_{w, j})}_{c d}$ 27

where $\frac{\partial Ω_{i, j}}{\partial ϵ_{i, k}} \in ℝ^{3 \times 3}, \frac{\partial \log (R)}{\partial (R)} \in ℝ^{(3 \times 3) \times (3 \times 3)}, \frac{\partial R^{- 1}}{\partial R} \in ℝ^{3 \times 3}$ , and $\frac{\partial R_{w, i}}{\partial ϵ_{i, k}} \in ℝ^{3 \times 3}$ . As a property of SO(3) matrices, $R_{i, w} = {(R_{w, i})}^{- 1} = {(R_{w, i})}^{⊤}$ . (For SE(3) matrices, T_i,w = (T_w,i)^–1.) The expression for the pose at a given time includes $\exp ({\tilde{B}}_{j} Ω_{i, j})$ , where the scalar ${\tilde{B}}_{j}$ is the j-th element of B at that time, as defined in Equation (5). The derivative of $\exp ({\tilde{B}}_{j} Ω_{i, j})$ with respect to the k-th element for applying the update vector є_j to control point R_w,j can be expressed as follows:

${(\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ϵ_{j, k}})}_{a b} = {\tilde{B}}_{j} {(\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ({\tilde{B}}_{j} Ω_{i, j})})}_{a b c d} {(\frac{\partial Ω_{i, j}}{\partial ϵ_{j, k}})}_{c d}$ 28

where $\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}$ . Similarly, the derivative of $\exp ({\tilde{B}}_{j} Ω_{i, j})$ with respect to the k-th element for applying the update vector є_j to control point R_w,j can be expressed as follows:

${(\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ϵ_{i, k}})}_{a b} = {\tilde{B}}_{j} {(\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ({\tilde{B}}_{j} Ω_{i, j})})}_{a b c d} {(\frac{\partial Ω_{i, j}}{\partial ϵ_{i, k}})}_{c d}$ 29

where $\frac{\partial \exp ({\tilde{B}}_{j} Ω_{i, j})}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}$ . Now, expressions have been presented for all of the prerequisite derivatives for expressing the derivative of a pose on the spline with respect to exponential updates to each of the control points.

A.2 Derivative of the Attitude on the Spline With Respect to Updates to the Control Points

For the attitude R_w,t at time t on the spline, the following expressions give the derivatives of the pose with respect to exponential updates to each of the control points dictating the spline on the interval t_i ≤ t < t_i+1:

$\begin{matrix} \frac{\partial R_{w, t}}{\partial ϵ_{i - 1, j}} = \frac{\partial R_{w, i - 1}}{\partial ϵ_{i - 1, j}} \exp ({\tilde{B}}_{1} Ω_{i - 1, i}) \exp ({\tilde{B}}_{2} Ω_{i, i + 1}) \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}) \\ + R_{w, i - 1} (\frac{\partial}{\partial ϵ_{i - 1, j}} \exp ({\tilde{B}}_{1} Ω_{i - 1, i})) \exp ({\tilde{B}}_{2} Ω_{i, i + 1}) \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}) \end{matrix}$ 30

$\begin{matrix} \frac{\partial R_{w, t}}{\partial ϵ_{i, j}} = R_{w, i - 1} (\frac{\partial}{\partial ϵ_{i, j}} \exp ({\tilde{B}}_{1} Ω_{i - 1, i})) \exp ({\tilde{B}}_{2} Ω_{i, i + 1}) \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}) \\ + R_{w, i - 1} \exp ({\tilde{B}}_{1} Ω_{i - 1, i}) (\frac{\partial}{\partial ϵ_{i, j}} \exp ({\tilde{B}}_{2} Ω_{i, i + 1})) \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}) \end{matrix}$ 31

$\begin{matrix} \frac{\partial R_{w, t}}{\partial ϵ_{i + 1, j}} = R_{w, i - 1} \exp ({\tilde{B}}_{1} Ω_{i - 1, i}) (\frac{\partial}{\partial ϵ_{i + 1, j}} \exp ({\tilde{B}}_{2} Ω_{i, i + 1})) \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}) \\ + R_{w, i - 1} \exp ({\tilde{B}}_{1} Ω_{i - 1, i}) \exp ({\tilde{B}}_{2} Ω_{i, i + 1}) (\frac{\partial}{\partial ϵ_{i + 1, j}} \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2})) \end{matrix}$ 32

$\frac{\partial R_{w, t}}{\partial ϵ_{i + 2, j}} = R_{w, i - 1} \exp ({\tilde{B}}_{1} Ω_{i - 1, i}) \exp ({\tilde{B}}_{2} Ω_{i . i + 1}) (\frac{\partial}{\partial ϵ_{i + 2, j}} \exp ({\tilde{B}}_{3} Ω_{i + 1, i + 2}))$ 33

These expressions arise from applying the product rule to the expression for the pose along the spline in Equation (5). For optimization with accelerometer and gyroscope measurements, the derivatives of the first and second time derivatives of the pose on the spline are also necessary. These derivatives are found by applying the product rule in the same manner to Equations (6) and (7) to find $\frac{\partial {\dot{R}}_{w, t}}{\partial ϵ_{j, k}}$ and $\frac{\partial {\ddot{R}}_{w, t}}{\partial ϵ_{j, k}}$ respectively.

B | JACOBIANS OF THE ACCELEROMETER MEASUREMENT MODEL

This section provides expressions for the Jacobian matrices of the residual errors in Equation (12). With a split-spline representation in SO(3) × ℝ³, the Jacobian matrix of the residual errors in Equation (12) with respect to index k of an exponential update vector applied to the SO(3) control point R_w,j is as follows:

$\frac{\partial}{\partial ϵ_{j, k}} h_{a} (t) = \frac{\partial R_{w, t}^{⊤}}{\partial ϵ_{j, k}} ({\ddot{p}}_{w} + 2 ω_{w, e i} \times {\dot{p}}_{w} - g_{w}) + R_{w, t}^{⊤} (\frac{\partial {\ddot{p}}_{w}}{\partial ϵ_{j, k}} + \frac{\partial {\dot{p}}_{w}}{\partial ϵ_{j, k}})$ 34

For this, we must first calculate $\frac{\partial R_{w, t}^{⊤}}{\partial ϵ_{j}}$ , the Jacobian of the transpose of the attitude sampled on the spline at time t with respect to an exponential update applied to R_w,j. The Jacobian of the residual errors in Equation (12) with respect to index k of an ℝ³ update vector added to position control point p_j is as follows:

$\frac{\partial}{\partial ϵ_{j, k}} h_{a} (t) = R_{w, t}^{⊤} (\frac{\partial {\ddot{p}}_{w}}{\partial ϵ_{j, k}} + \frac{\partial {\dot{p}}_{w}}{\partial ϵ_{j, k}})$ 35

This step requires first calculating $\frac{\partial {\ddot{p}}_{w}}{\partial ϵ_{j}}$ , the Jacobian of the second time derivative of the position on the spline at time t with respect to an update vector applied to control point p_j, and $\frac{\partial {\dot{p}}_{w}}{\partial ϵ_{j}}$ , the Jacobian of the first time derivative of the position on the spline at time t with respect to an update vector applied to control point p_j . For a trajectory represented by a spline in SE(3) and with the derivatives of the pose and time derivatives of the pose along the spline at time t with respect to index k of an exponential update vector applied to control point j, the Jacobian of the high-precision accelerometer measurement model is as follows:

where $\frac{\partial}{\partial ϵ_{j, k}} h_{a} (t) \in ℝ^{3}, \frac{\partial R_{w, t}}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}$ is the upper-left 3 × 3 submatrix of $\frac{\partial T_{w, t}}{\partial ϵ_{j, k}}, \frac{\partial {\ddot{p}}_{w, t}}{\partial ϵ_{j, k}} \in ℝ^{3}$ is composed of the first three rows of the last column of $\frac{\partial {\ddot{T}}_{w, t}}{\partial ϵ_{j, k}}$ (i.e., the indices of the acceleration vector), and $\frac{\partial {\ddot{p}}_{w, t}}{\partial ϵ_{j, k}} \in ℝ^{3}$ is composed of first three rows of the last column of $\frac{\partial {\dot{T}}_{w, t}}{\partial ϵ_{j, k}}$ (i.e., the indices of the velocity vector). The Jacobian matrix of the expected accelerometer measurement with respect to updates to the estimated accelerometer bias state, b_a, is simply the I₃ matrix.

C | JACOBIANS OF THE GYROSCOPE MEASUREMENT MODEL

This section provides expressions for the Jacobian matrices of the residual errors in Equation (13). With a split-spline representation in SO(3) × ℝ³, the Jacobian matrix of the residual errors in Equation (13) with respect to index k of an exponential update vector applied to SO(3) control point R_w,j is as follows:

$\frac{\partial}{\partial ϵ_{j, k}} h_{g} (t) = {(\frac{\partial R_{w, t}^{⊤}}{\partial ϵ_{j, k}} {\dot{R}}_{w, t} + R_{w, t}^{⊤} \frac{\partial {\dot{R}}_{w, t}}{\partial ϵ_{j, k}})}^{\lor} + \frac{\partial R_{w, t}^{⊤}}{\partial ϵ_{j, k}} ω_{b, e i}$ 37

For this, we must first calculate $\frac{\partial R_{w, t}^{⊤}}{\partial ϵ_{j}}$ , the Jacobian of the transpose of the attitude sampled on the spline at time t with respect to an exponential update applied to R_w,j, and $\frac{\partial {\dot{R}}_{w, t}}{\partial ϵ_{j}}$ , the Jacobian of the first time derivative of the attitude sampled on the spline at time t with respect to an exponential update applied to R_w,j. For a trajectory represented by a spline in SE(3) and with the derivatives of the pose and first time derivative of the pose along the spline at time t with respect to index k of an exponential update vector applied to control point j, the Jacobian of the high-precision gyroscope measurement model is as follows:

where $\frac{\partial}{\partial ϵ_{j, k}} h_{g} (t) \in ℝ^{3}, \frac{\partial R_{w, t}}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3},$ is the upper-left submatrix of $\frac{\partial T_{w, t}}{\partial ϵ_{j, k}}$ , and $\frac{\partial {\dot{R}}_{w, t}}{\partial ϵ_{j, k}} \in ℝ^{3 \times 3}$ is the upper-left submatrix of $\frac{\partial {\dot{T}}_{w, t}}{\partial ϵ_{j, k}}$ . The Jacobian matrix of the expected gyroscope measurement with respect to updates to the estimated gyroscope bias state, b_g, is the I₃ matrix.

D | SOFTWARE

The approaches in this paper were developed in Python 3.10.5. The implementation and evaluation depend on the following publicly available Python libraries: r3f 2.0.14, SciPy 1.8.1, NumPy 1.21.6, Matplotlib 3.5.2, pandas 1.4.3, and Numba 0.61.0. The r3f library is for reference frame rotations. SciPy’s “sparse” library was used for iteratively forming and solving the associated linearized least-squares problems. NumPy, Matplotlib, and pandas were used for quotidian utilities in implementation and evaluation. Numba was used for “just-in-time” compiling to expedite run times. Code to implement the approach and evaluations described in this paper can be found in the GitHub repository: https://github.com/kaleland/spline-based-factor-graph-optimization-with-high-grade-inertial-sensors/tree/main.

Footnotes

↵1 The two trajectory representations relevant to this paper are (1) a split representation in SO(3) × ℝ³, which includes a spline in the Lie group SO(3) to represent attitude and a spline in ℝ³ to represent position, and (2) a unified spline in SE(3) that represents both attitude and position. We postpone a comparison of these approaches to Section 4.
↵2 A comparison between pre-integrated inertial measurements and raw inertial measurements in a spline-based factor graph would be valuable, but is beyond the scope of this work.

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

↵
Anderson, S., Barfoot, T. D., Tong, C. H., & Särkkä, S. (2015). Batch nonlinear continuous-time trajectory estimation as exactly sparse Gaussian process regression. Autonomous Robots, 39(3), 221–238. https://doi.org/10.1007/s10514-015-9455-y
Google Scholar
↵
Blanco, J.-L. (2010). A tutorial on SE(3) transformation parameterizations and on-manifold optimization (tech. rep.). University of Malaga.
Google Scholar
↵
Brody, J., Leung, S., Shamwell, J., & Donavanik, D. (2023). Sources of error in GPS-denied odometry (tech. rep.). U.S. Army Combat Capabilities Development Command.
Google Scholar
↵
Cox, M. G. (1972). The numerical evaluation of B-splines. IMA Journal of Applied Mathematics, 10(2), 134–149. https://doi.org/10.1093/imamat/10.2.134
Google Scholar CrossRef
↵
de Boor, C. (1978). A practical guide to splines (1st ed., Vol. 27). Springer-Verlag. https://link.springer.com/book/9780387953663#accessibility-information
Google Scholar
↵
Dellaert, F., & Kaess, M. (2006). Square root SAM: Simultaneous localization and mapping via square root information smoothing. The International Journal of Robotics Research, 25(12), 1181–1203. https://doi.org/10.1177/0278364906072768
Google Scholar CrossRef Web of Science
↵
Dennis, J. E., & Schnabel, R. B. (1996). Numerical methods for unconstrained optimization and nonlinear equations. SIAM.
Google Scholar
↵
Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2017). On-manifold preintegration for real-time visual–inertial odometry. IEEE Transactions on Robotics, 33(1), 1–21. https://doi.org/10.1109/tro.2016.2597321
Google Scholar
↵
Furgale, P., Barfoot, T. D., & Sibley, G. (2012). Continuous-time batch estimation using temporal basis functions. 2012 IEEE International Conference on Robotics and Automation, 2088–2095. https://doi.org/10.1109/ICRA.2012.6225005
Google Scholar
↵
Hiatt, J., & Taylor, C. N. (2022). A comparison of correlation-agnostic techniques for magnetic navigation. Proc. of the 2022 25th International Conference on Information Fusion (FUSION), 1–7. https://doi.org/10.23919/FUSION49751.2022.9841293
Google Scholar
↵
Hug, D., Bänninger, P., Alzugaray, I., & Chli, M. (2022). Continuous-time stereo-inertial odometry. IEEE Robotics and Automation Letters, 7(3), 6455–6462. https://doi.org/10.1109/LRA.2022.3173705
Google Scholar
↵
Kschischang, F., Frey, B., & Loeliger, H.-A. (2001). Factor graphs and the sum-product algorithm. IEEE Transactions on Information Theory, 47(2), 498–519. https://doi.org/10.1109/18.910572
Google Scholar
↵
Lv, J., Hu, K., Xu, J., Liu, Y., Ma, X., & Zuo, X. (2021). CLINS: Continuous-time trajectory estimation for LiDAR-inertial system. 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 6657–6663. https://doi.org/10.1109/IROS51168.2021.9636676
Google Scholar
↵
Marquardt, D. W. (1963). An algorithm for least-squares estimation of nonlinear parameters. Journal of the Society for Industrial and Applied Mathematics, 11(2), 431–441.
Google Scholar CrossRef Web of Science
↵
Mueggler, E., Gallego, G., & Scaramuzza, D. (2015). Continuous-time trajectory estimation for event-based vision sensors. Robotics: Science and Systems, 11, 36–44. https://doi.org/10.15607/RSS.2015.XI.036
Google Scholar
↵
Ng, Y. Z., Choi, B., Tan, R., & Heng, L. (2021). Continuous-time radar-inertial odometry for automotive radars. 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 323–330. https://doi.org/10.1109/IROS51168.2021.9636014
Google Scholar
↵
Oth, L., Furgale, P., Kneip, L., & Siegwart, R. (2013). Rolling shutter camera calibration. 2013 IEEE Conference on Computer Vision and Pattern Recognition, 1360–1367. https://doi.org/10.1109/CVPR.2013.179
Google Scholar
↵
Patron-Perez, A., Lovegrove, S., & Sibley, G. (2015). A spline-based trajectory representation for sensor fusion and rolling shutter cameras. International Journal of Computer Vision, 113. https://doi.org/10.1007/s11263-015-0811-3
Google Scholar
↵
Qin, K. (1998). General matrix representations for B-splines. Proc. of the Pacific Graphics ‘98. Sixth Pacific Conference on Computer Graphics and Applications (Cat. No.98EX208), 37–43. https://doi.org/10.1109/PCCGA.1998.731996
Google Scholar
↵
Sommer, C., Usenko, V., Schubert, D., Demmel, N., & Cremers, D. (2020). Efficient derivative computation for cumulative B-splines on Lie groups. 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 11145–11153. https://doi.org/10.1109/cvpr42600.2020.01116
Google Scholar
↵
Sommer, H., Forbes, J. R., Siegwart, R., & Furgale, P. (2016). Continuous-time estimation of attitude using B-splines on Lie groups. Journal of Guidance, Control, and Dynamics, 39(2), 242–261. https://doi.org/10.2514/1.G001149
Google Scholar
↵
Tang, H., Zhang, T., Niu, X., Fan, J., & Liu, J. (2022). Impact of the earth rotation compensation on MEMS-IMU preintegration of factor graph optimization. IEEE Sensors Journal, 22(17), 17194–17204. https://doi.org/10.1109/JSEN.2022.3192552
Google Scholar
↵
Taylor, C., & Gross, J. (2024). Factor graphs for navigation applications: A tutorial. NAVIGATION, 71(3). https://doi.org/10.33012/navi.653
Google Scholar
↵
Titterton, D., & Weston, J. (2004). Strapdown inertial navigation technology (2nd ed.). The Institution of Engineering; Technology. https://doi.org/10.1049/PBRA017E
Google Scholar
↵
Trefethen, L. N., & Bau, D. (2022). Numerical linear algebra, twenty-fifth anniversary edition. Society for Industrial; Applied Mathematics. https://doi.org/10.1137/1.9781611977165
Google Scholar
↵
Woodburn, D. (2023a). Inertial navigation [Unpublished manuscript, Air Force Institute of Technology].
Google Scholar
↵
Woodburn, D. (2023b). INU: Inertial navigation utilities (version 1.0.8) [Version released November 22, 2023. Accessed January 2025].
Google Scholar

PDF

Previous Next

Spline-Based Factor-Graph Optimization with High-Grade Inertial Sensors

Abstract

1 INTRODUCTION