Uncertainty Propagation for Inertial Navigation with Coning, Sculling, and Scrolling Corrections

Abstract

This paper investigates the propagation of estimation errors through a common coning, sculling, and scrolling architecture used in modern-day inertial navigation systems. Coning, sculling, and scrolling corrections often have an unaccounted for effect on the error statistics of inertial measurements used to describe the state and uncertainty propagation for position, velocity, and attitude estimates. Through the development of an error analysis for a set of coning, sculling, and scrolling algorithms, mappings of the measurement and estimation errors through the correction term are adaptively generated. Using the developed mappings, an efficient and consistent propagation of the state and uncertainty, within the multiplicative extended Kalman filter architecture, is achieved. Monte Carlo analysis is performed, and results show that the developed system has favorable attributes when compared to the traditional mechanization.

1. Introduction

Inertial navigation describes the technique whereby the integration of non-gravitational specific force and total angular rate measurements are used in conjunction with a gravity model to produce estimates for the vehicle states: i.e., the inertial position, velocity, and attitude. In combining the modern inertial navigation system (INS) with the extended Kalman filter (an extension to the Kalman filter [1] for nonlinear systems), estimates for these states and a measure of uncertainty in each can be maintained. Before the introduction of strapdown sensors, inertially stabilized platforms (ISPs) were used to maintain the vehicle’s navigation frame and isolate the necessary sensors from the body’s rotation and any present vibration. A few examples of ISP mechanizations of the INS include the Apollo PGNCS [2], which served on both the command module and lunar module, and those developed for the Minuteman III and Peacekeaper intercontinental ballistic missiles [3]. Though ISPs were the standard until late in the 20th century, they have been widely replaced by the strapdown mechanization, where the sensors are instead “strapped down” or attached directly to the structure of the vehicle [4]. The introduction of strapdown inertial navigation systems (SINSs) significantly reduced the mass and complexity of the INS, as compared to the ISPs, by removing auxiliary components required for the housing and stabilization of ISPs. An early adoption of the SINS for space missions includes the Apollo Abort Guidance System in 1969 as a backup to the PGNCS, where the added mass of a second ISP became a significant consideration [5]. Unfortunately, with the adoption of the SINS, it became necessary to computationally maintain the transformation to the inertially fixed navigation frame as the SINS is not mechanically isolated from the vehicle rotation.

Since the adoption of SINSs, maintaining an accurate attitude estimate has become integral to velocity and position estimation. However, when out-of-phase sinusoidal motion is exhibited about two orthogonal axes, significant error growth is seen [6]—this is commonly referred to as coning motion because, when the angular velocity magnitude and frequency of oscillation are constant, the third axis appears to move on the surface of a cone. These effects are commonly seen in vibrational environments or when maneuvers are performed and are of particular interest to many aerospace applications [7]. A significant portion of SINS development initially focused on generating efficient algorithms capable of addressing the attitude error growth due to coning [8,9,10,11]. The first generation of modern coning correction algorithms was developed by making basic dynamical assumptions and utilizing a sequence of high-frequency inertial measurements [12,13,14], while newer approaches seek to minimize the error under specific circumstances [15,16,17]. Errors can also occur due to sculling and scrolling motions, which are the velocity and position analogs to coning motion, respectively, though they tend not to have the impact that coning does [14]. Fortunately, it has also been shown that once an optimal coning algorithm has been developed, a dual approach for sculling can also be obtained [18,19], extending the usefulness of the approaches. The development of coning, sculling, and scrolling (CSS) algorithms has been critical to SINS-aided navigation, mitigating the effects of cascading attitude errors into the integrated velocity and position. A generalized set of CSS algorithms presented by Savage in [13,14] and their application to modern inertial navigation systems are of primary interest in this paper.

The modern INS is comprised of three orthogonal linear accelerometers and gyroscopes that measure non-gravitational acceleration and total angular velocity, while a navigation computer is employed to integrate these measurements. Because measurements from these sensors help to describe a subset of the vehicle dynamics and are self-contained, they are often called internal or inertial sensors—this helps to differentiate them from other critical measurement systems. Before being passed to the navigation computer, some combination of the CSS corrections is typically applied to the measurements along the processing chain, either within a high-frequency pre-processor or the manufacturer’s sensor software [20,21]. The CSS-corrected measurements are passed to the navigation computer for state propagation. The extended Kalman filter typically serves as the backbone of many modern navigation systems—to accurately propagate the uncertainty for inertial navigation, suitable models for the dynamics and inertial measurements must be known. Unfortunately, inertial measurements are often corrupted by many parametric sensor errors that can have a significant impact on navigation performance [22]. Often, estimates for each of the parametric states are used to correct the inertial measurements before integrating [23]. Thus, the use of CSS-corrected quantities that have been corrected for parametric sensor state estimates contribute to the navigation uncertainty, though this contribution typically goes unaccounted for. By developing and implementing an error mapping through the algorithms used for CSS corrections, a rigorous treatment of these errors and their potential effect on the state uncertainty can be realized.

The remainder of the paper is summarized as follows: Section 2 reviews the mechanics of inertial navigation and presents a standard set of algorithms to make second-order CSS corrections. The primary contribution of the work is contained in Section 3, which outlines the development of the error propagation and presents the mappings for use within the linearized uncertainty propagation. Section 4 describes the approaches to be compared, the lunar descent-to-landing scenario in which the developed methods are examined, and the performance metrics used in analysis. Monte Carlo simulation results are then examined in Section 5. Finally, Section 6 briefly summarizes the work and its primary conclusions.

2. Inertial Navigation

Modern CSS algorithms require several operational frequencies, here defined to be the major, minor, and subminor time intervals illustrated in Figure 1. The subminor interval $[t_{m-1},t_m]$ describes the time over which the inertial sensors integrate to provide discrete measurements of the non-gravitational acceleration and total vehicle angular velocity. The minor interval $[t_{\ell -1},t_{\ell }]$ defines the rate at which the high-speed correction algorithms are applied, and the major interval $[t_{k-1},t_k]$ defines the navigation rate, or the rate at which the state propagation is performed. For the algorithms considered throughout this paper, the assumption is made that $[t_{m-1},t_m]=[t_{\ell -1},t_{\ell }]\subseteq [t_{k-1},t_k]$, such that the minor and subminor intervals are equivalent.

Figure 1. Major, minor, and subminor time intervals considered by the algorithms.

The CSS algorithms inspected within this paper generate second-order corrections for coning, sculling, and scrolling motion and are based upon those discussed by Savage in [4,13,14]. These algorithms, along with many other CSS algorithms, were developed after Bortz presented a differential equation of the orientation vector (also called the rotation vector), given as [11]

$$\begin{array}{c}\hfill \dot{\mathbf{\varphi }}=\mathbf{\omega }+\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi sin\varphi }{2(1-cos\varphi )}\right)\mathbf{\varphi }\times \mathbf{\varphi }\times \mathbf{\omega },\end{array}$$

(1)

where $\mathbf{\varphi }$ is the orientation vector describing the rotation of one frame to another through an angle, $\varphi =\parallel \mathbf{\varphi }\parallel$, about an axis pointing in the direction of $\mathbf{\varphi }$, and $\mathbf{\omega }$ is the angular velocity of the body that is inertially measurable by strapdown angular-rate sensors. Equation (1) is commonly referred to as the Bortz equation and allows for the integration of the orientation vector using measurements from strapdown sensors. Many of the coning algorithms originate from the isolation and approximation of the non-commutative rate vector, $\dot{\mathbf{\alpha }}$, in the Bortz equation,

$$\begin{array}{c}\hfill \dot{\mathbf{\alpha }}=\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi sin\varphi }{2(1-cos\varphi )}\right)\mathbf{\varphi }\times \mathbf{\varphi }\times \mathbf{\omega }.\end{array}$$

(2)

A common approximation for Equation (2) is obtained from a power series expansion for the coefficient of the second term in Equation (2), such that

$$\begin{array}{c}\hfill \frac{1}{{\varphi }^2}\left(1-\frac{\varphi sin\varphi }{2(1-cos\varphi )}\right)=\frac{1}{12}\left(1+\frac{1}{60}{\varphi }^2+\cdots \right)\approx \frac{1}{12},\end{array}$$

(3)

which allows Equation (1) to be approximated as

$$\begin{array}{c}\hfill \dot{\mathbf{\varphi }}\approx \mathbf{\omega }+\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{12}\mathbf{\varphi }\times \left(\mathbf{\varphi }\times \mathbf{\omega }\right).\end{array}$$

(4)

The use of Equation (4) as an approximation for Equation (1) to compensate for coning motion in a two-stage algorithm, which generates the correction at a higher frequency than the state propagation, by Bortz [11] and Jordan [10] laid the foundation for modern coning algorithms. These two-stage approaches generate high-frequency, low-complexity corrections, the results of which are fed into a low-frequency algorithm that produces a state estimate. One of the original two-stage algorithms proposed by Savage in 1966 utilizes a first-order equation at a higher frequency to recognize high-frequency vibrations and a second-order attitude update at a lower frequency, providing an efficient and accurate attitude estimate based upon the corrections made by the high-frequency algorithm [8]. While the two-stage approach was originally introduced because of limited computer capabilities, modern computing capabilities have prompted the desire to return to a single cycle algorithm [13]. However, the algorithms described for most of the two-stage algorithms can also be expanded instead to process a batch of sequential measurements to produce an equivalent, coned measurement at a lower frequency.

The algorithms considered within this paper represent a generalized form of the corrections, assuming that the quantities vary linearly over the minor interval. Additionally, the algorithms examined here are made using only two measurements such that the subminor and minor intervals are the same. Many modern coning correction algorithms have been optimized for error minimization, dependent upon the expected environment or intended number of measurements [12,15,24,25]. It is also important to realize that sculling and scrolling algorithm design has seen much less research and development, leading to a significantly smaller body of literature examining their use. By performing an analysis of the error propagation through unoptimized algorithms, the foundation for analyzing and developing an error propagation architecture for other CSS algorithms used for inertial navigation is established. A detailed derivation of the algorithms is provided by Savage in [4,13,14] and will not be provided within this paper.

2.1. Attitude Integration (Coning) Algorithm

The coning algorithm generates a second-order approximation for the coning motion, where the incremental angle vector produced by the inertial measurement unit (IMU) gyroscopes is

$$\begin{array}{c}\hfill \Delta {\mathbf{\theta }}_m^c={\int }_{t_{m-1}}^{t_m}{\mathbf{\omega }}_{c/i}^c\left(\tau \right)\mathrm{d}\tau ,\end{array}$$

(5)

where ${\mathbf{\omega }}_{c/i}^c$ is the angular velocity vector of the IMU case frame (denoted by c) with respect to the inertial frame (denoted by i). This incremental angle vector describes the rotation of the IMU case frame from $t_{m-1}$ to $t_m$, where the superscript c denotes the expression of the vector in the IMU case frame. The frame designation will be implied for the coning elements from here on, as the frames will be consistent throughout the remainder of the paper.

Accumulations for the measurements and corrections made at the end of every minor interval, contained within the major interval, are defined as a function of the incremental angle measurements and used to produce the coned rotation vector. The measurement accumulation is given by

$$\begin{array}{cc}\hfill {\mathbf{\theta }}_{\ell }& ={\mathbf{\theta }}_{\ell -1}+\Delta {\mathbf{\theta }}_{\ell }=\sum _{i=1}^{\ell }\Delta {\mathbf{\theta }}_i.\hfill \end{array}$$

(6)

The accumulation of the coning corrections is expressed similarly to Equation (6) as

$$\begin{array}{cc}\hfill {\mathbf{\beta }}_{\ell }& ={\mathbf{\beta }}_{\ell -1}+\Delta {\mathbf{\beta }}_{\ell }=\sum _{i=1}^{\ell }\Delta {\mathbf{\beta }}_i,\hfill \end{array}$$

(7)

where $\Delta {\mathbf{\beta }}_i$ is the coning correction generated at $t_i$ under the assumption of a linearly varying angular velocity; i.e.,

$$\begin{array}{c}\hfill \Delta {\mathbf{\beta }}_i=\frac{1}{2}\left[{\mathbf{\theta }}_{i-1}+\frac{1}{6}\Delta {\mathbf{\theta }}_{i-1}\right]\times \Delta {\mathbf{\theta }}_i.\end{array}$$

Given the accumulations in Equations (6) and (7), the coned rotation vector $\Delta {\mathbf{\varphi }}_k$ is the sum of the two accumulations, or

$$\begin{array}{c}\hfill \Delta {\mathbf{\varphi }}_k={\mathbf{\theta }}_{\ell }+{\mathbf{\beta }}_{\ell },\end{array}$$

(8)

where ${\mathbf{\theta }}_{\ell }$ describes the sensed change in the attitude over the $[t_{k-1},t_k]$ interval and ${\mathbf{\beta }}_{\ell }$ accounts for the non-commutative and unmeasured components due to the changing angular velocity vector. The k subscript denotes that this is the coned rotation vector describing the rotation from $t_{k-1}$ to $t_k$, while the ℓ subscript describes the number of measurements and corrections accumulated within the major interval.

At the initialization of the algorithm for any given major interval, the terms from the previous time-step must be zero ($\Delta {\mathbf{\theta }}_0=\mathbf{0}$ and $\Delta {\mathbf{\beta }}_0=\mathbf{0}$ at $t_0=t_{k-1}$). Note that any number of measurements can be processed between the attitude predictions, but when $[t_{\ell -1},t_{\ell }]=[t_{k-1},t_k]$, or just a single measurement is processed for the prediction, the algorithm becomes identical to traditional methods of dead-reckoning [26], where only a single IMU measurement is processed for the attitude propagation at each step; this statement can be proven by recognizing that with the initialization of the accumulation variables to zero, the correction will also be zero. Finally, if the angular velocity vector is constant in direction, there is no coning motion, the cross product will be zero, and the coning correction in each measurement will thus be zero.

2.2. Velocity Integration (Sculling) Algorithm

The velocity integration algorithms correct for errors incurred by IMU frame and velocity vector rotations. The algorithm uses the incremental velocity measurement from the IMU, a quantification of the non-gravitational specific forces acting on the vehicle, given by

$$\begin{array}{cc}\hfill \Delta v_m^c& ={\int }_{t_{m-1}}^{t_m}{\mathbf{a}}_{ng}^c\left(\tau \right)\mathrm{d}\tau ,\hfill \end{array}$$

where ${\mathit{a}}_{ng}^c$ is the non-gravitational specific force experienced by the strapdown IMU. Note that the superscript c denotes the expression of the incremental velocity in the case frame of the IMU; this subscript will also be implied for the sculling elements for the remainder of the paper.

Similar to the accumulation of the measurements in Equation (6), the incremental velocity measurements must also be accumulated, such that

$$\begin{array}{c}\hfill v_{\ell }=v_{\ell -1}+\Delta v_{\ell }=\sum _{i=1}^{\ell }\Delta v_i.\end{array}$$

(9)

The sculling-corrected incremental velocity, describing the change in velocity due to the non-gravitational specific forces acting on the vehicle from $t_{k-1}$ to $t_k$, can then be separated into three components,

$$\begin{array}{c}\hfill \Delta v_{ng,k}=v_{\ell }+\Delta v_{scul,\ell }+\Delta v_{rot,\ell },\end{array}$$

(10)

where $\Delta v_{scul,\ell }$ is the sculling correction and $\Delta v_{rot,\ell }$ is the compensation for the rotation of the velocity vector. The k and ℓ subscripts describe the dependency of the sculled non-gravitational change in velocity upon the ℓ inertial measurements obtained over the major interval. The sculling correction can be modeled as an accumulation

$$\begin{array}{cc}\hfill \Delta v_{scul,\ell }& =\Delta v_{scul,\ell -1}+\delta v_{scul,\ell }=\sum _{i=1}^{\ell }\delta v_{scul,i},\hfill \end{array}$$

(11)

where the incremental sculling correction is given by

$$\begin{array}{cc}\hfill \delta v_{scul,i}& =\frac{1}{2}\left[\left({\mathbf{\theta }}_{i-1}+\frac{1}{6}\Delta {\mathbf{\theta }}_{i-1}\right)\times \Delta v_i+\left(v_{i-1}+\frac{1}{6}\Delta v_{i-1}\right)\times \Delta {\mathbf{\theta }}_i\right],\hfill \end{array}$$

(12)

which results from an integration of the sculling motion over the measurement cycle, assuming a linearly changing angular velocity and specific force [14]. The correction due to the rotation of the velocity vector on the interval is

$$\begin{array}{c}\hfill \Delta v_{rot,\ell }=\frac{1}{2}\left({\mathbf{\theta }}_{\ell }\times v_{\ell }\right),\end{array}$$

(13)

where ${\mathbf{\theta }}_{\ell }$ is the accumulation of the incremental angle measurements as discussed in Section 2.1. The non-gravitational incremental velocity is then used for the vehicle’s velocity propagation.

At the initialization of the algorithm, for any given major interval, the terms from the previous time-step must be zero ($v_0=\mathbf{0}$ and $\Delta v_{scul,0}=\mathbf{0}$ at $t_0=t_{k-1}$) because no information is available for the correction on the current interval. Similarly to the coning algorithm, this algorithm can be used to process any number of measurements, and when $[t_{\ell -1},t_{\ell }]=[t_{k-1},t_k]$, or just a single measurement is used for state prediction, the algorithm becomes identical to the traditional method of dead-reckoning at measurement frequency. This is proven similar to the equivalent statement regarding the coning algorithm.

2.3. Position Integration (Scrolling) Algorithm

No additional measurement source is used for the position integration algorithm; the integrated specific force is again integrated to provide the position increment, while the scrolling algorithm corrects for the effects of a varying angular rate and specific forces upon the integration. The effects of scrolling can be accounted for in the non-gravitational specific force integration, given by

$$\begin{array}{c}\hfill \Delta {\mathit{r}}_{ng,k}={\mathit{s}}_{v,\ell }+\Delta {\mathit{r}}_{rot,\ell }+\Delta {\mathit{r}}_{scrl,\ell }.\end{array}$$

(14)

The accumulation of the integrated incremental velocity is defined to be

$$\begin{array}{cc}\hfill {\mathit{s}}_{v,\ell }& ={\mathit{s}}_{v,\ell -1}+\Delta {\mathit{s}}_{v,\ell }=\sum _{i=1}^{\ell }\Delta {\mathit{s}}_{v,i},\hfill \end{array}$$

(15)

where

$$\begin{array}{c}\hfill \Delta {\mathit{s}}_{v,i}=v_{i-1}\Delta t_{\ell }+\frac{1}{12}\left(5\Delta v_i+\Delta v_{i-1}\right)\Delta t_{\ell }\end{array}$$

(16)

and $\Delta t_{\ell }=t_{\ell }-t_{\ell -1}$. Note that the result in Equation (16) is obtained by integrating the specific force, assuming that the angular velocity and specific force vary linearly over the minor interval [4]. The rotational component of the scrolling correction is given by

$$\begin{array}{c}\hfill \Delta {\mathit{r}}_{rot,\ell }=\frac{1}{6}\left({\mathit{s}}_{\theta ,\ell }\times v_{\ell }+{\mathbf{\theta }}_{\ell }\times {\mathit{s}}_{v,\ell }\right),\end{array}$$

(17)

with the integrated incremental angle accumulating as

$$\begin{array}{cc}\hfill {\mathit{s}}_{\theta ,\ell }& ={\mathit{s}}_{\theta ,\ell -1}+\Delta {\mathit{s}}_{\theta ,\ell }=\sum _{i=1}^{\ell }\Delta {\mathit{s}}_{\theta ,i},\hfill \end{array}$$

where

$$\begin{array}{c}\hfill \Delta {\mathit{s}}_{\theta ,i}={\mathbf{\theta }}_{i-1}\Delta t_{\ell }+\frac{1}{12}\left(5\Delta {\mathbf{\theta }}_i+\Delta {\mathbf{\theta }}_{i-1}\right)\Delta t_{\ell }.\end{array}$$

The scrolling correction can be broken into a component accounting for the effects due to sculling and a component accounting for other higher-order effects. The accumulation of these effects is therefore given by

$$\begin{array}{c}\hfill \Delta {\mathit{r}}_{scrl,i}=\Delta {\mathit{r}}_{scrl,i-1}+\delta {\mathit{r}}_{scrl/scul,i}+\delta {\mathit{r}}_{scrl/other,i},\end{array}$$

(18)

where the scrolling correction contributed by sculling is

$$\begin{array}{cc}\hfill \delta {\mathit{r}}_{scrl/scul,i}=& \Delta v_{scul,i-1}\Delta t_{\ell }+\frac{1}{2}\left[{\mathbf{\theta }}_{i-1}-\frac{1}{12}\left(\Delta {\mathbf{\theta }}_i-\Delta {\mathbf{\theta }}_{i-1}\right)\right]\times \left(\Delta {\mathit{s}}_{v,i}-v_{i-1}\Delta t_{\ell }\right)\hfill \\ \hfill & +\frac{1}{2}\left[v_{i-1}-\frac{1}{12}\left(\Delta v_i-\Delta v_{i-1}\right)\right]\times \left(\Delta {\mathit{s}}_{\theta ,i}-{\mathbf{\theta }}_{i-1}\Delta t_{\ell }\right)\hfill \end{array}$$

(19)

and the correction for the other higher-order effects is

$$\begin{array}{cc}\hfill \delta {\mathit{r}}_{scrl/other,i}=& \frac{1}{6}\left[{\mathit{s}}_{v,i-1}+\frac{\Delta t_{\ell }}{24}\left(\Delta v_i-\Delta v_{i-1}\right)\right]\times \Delta {\mathbf{\theta }}_i\hfill \\ \hfill & -\frac{1}{6}\left[{\mathit{s}}_{\theta ,i-1}+\frac{\Delta t_{\ell }}{24}\left(\Delta {\mathbf{\theta }}_i-\Delta {\mathbf{\theta }}_{i-1}\right)\right]\times \Delta v_i\hfill \\ \hfill & +\frac{\Delta t_{\ell }}{6}\left[{\mathbf{\theta }}_{i-1}-\frac{1}{6}\left(\Delta {\mathbf{\theta }}_i-\Delta {\mathbf{\theta }}_{i-1}\right)\right]\times \left[v_{i-1}-\frac{1}{6}\left(\Delta v_i-\Delta v_{i-1}\right)\right]\hfill \\ \hfill & -\frac{\Delta t_{\ell }}{2160}\left(\Delta {\mathbf{\theta }}_i-\Delta {\mathbf{\theta }}_{i-1}\right)\times \left(\Delta v_i-\Delta v_{i-1}\right).\hfill \end{array}$$

(20)

Similar to the coning and sculling algorithms, each term from the previous cycle must be initialized to zero (${\mathit{s}}_{\theta ,0}=\mathbf{0}$, ${\mathit{s}}_{v,0}=\mathbf{0}$, ${\mathbf{\theta }}_0=\mathbf{0}$, $\Delta {\mathbf{\theta }}_0=\mathbf{0}$, $v_0=\mathbf{0}$ and $\Delta v_i=\mathbf{0}$ at $t_0=t_{k-1}$). It can also be shown that the scrolling correction algorithm and the traditional method for position dead-reckoning are equivalent given that no scrolling motion is present.

2.4. Discrete Dead-Reckoning Dynamics

The discretized dynamics for a vehicle aided by a strapdown IMU may be expressed as

$$\begin{array}{cc}\hfill {\mathit{r}}_{c,k}^i=& {\mathit{r}}_{c,k-1}^i+v_{c,k-1}^i\Delta t_k+{\mathit{T}}_{c,k-1}^i\Delta {\mathit{r}}_{ng,k}\hfill \\ & +\frac{1}{2}\left(g_{k-1}-\frac{1}{3}{\mathit{G}}_{k-1}{\mathit{T}}_{c,k-1}^i\left[d_{k-1}^c\times \right]\Delta {\mathbf{\varphi }}_k\right)\Delta t_k^2\hfill \end{array}$$

(21a)

$$\begin{array}{cc}\hfill v_{c,k}^i=& v_{c,k-1}^i+{\mathit{T}}_{c,k-1}^i\Delta v_{ng,k}+\left(g_{k-1}-\frac{1}{2}{\mathit{G}}_{k-1}{\mathit{T}}_{c,k-1}^i\left[d_{k-1}^c\times \right]\Delta {\mathbf{\varphi }}_k\right)\Delta t_k\hfill \end{array}$$

(21b)

$$\begin{array}{c}{\overline{\mathit{q}}}_{i,k}^c=\overline{\mathit{q}}(\Delta {\mathbf{\varphi }}_k)\otimes {\overline{\mathit{q}}}_{i,k-1}^c,\hfill \end{array}$$

(21c)

describing the propagation of position $\mathit{r}$, velocity $v$ and attitude $\overline{\mathit{q}}$ from $t_{k-1}$ to $t_k$. Note that the superscripts c and i denote quantities described in the IMU case frame and inertially fixed reference frame, respectively, while the k and $k-1$ subscripts describe the discrete instances in time $t_k$ and $t_{k-1}$, respectively, where the value is available to the navigation computer. Consequently, time time between navigation cycles is thus defined to be $\Delta t_k=t_k-t_{k-1}$. The transformation from the inertial frame to the case frame is described by the right-handed vector-first attitude quaternion ${\overline{\mathit{q}}}_i^c$ and transformation matrix ${\mathit{T}}_i^c$—the transformation from the case to inertial frame is then given by the transpose of the transformation matrix, ${\left({\mathit{T}}_i^c\right)}^T={\mathit{T}}_c^i$. The acceleration acting on the vehicle due to gravity is denoted by $g={\mathit{a}}_g\left({\mathit{r}}_{cg}\right)$, and $\mathit{G}$ is the Jacobian of $g$ evaluated at vehicle center of gravity, ${\mathit{r}}_{cg}$, where ${\mathit{r}}_{cg}^i={\mathit{r}}_c^i+{\mathit{T}}_c^i{d}^c$ relates the position of the vehicle center of gravity ${\mathit{r}}_{cg}$ to the position of the IMU’s case frame ${\mathit{r}}_c$ via their relative position vector $d$. The quaternion multiplication is denoted by ⊗ and defined such that the order of quaternion multiplication parallels the multiplication of corresponding transformation matrices. The skew-symmetric cross product matrix is defined to be the matrix equivalent of the cross product operation; i.e., $\mathit{a}\times \mathit{b}=[\mathit{a}\times ]\mathit{b}$ where $\mathit{a},\mathit{b}\in {\mathbb{R}}^3$ and

$$\begin{array}{c}\hfill [\mathit{a}\times ]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right].\end{array}$$

The total rotation of the body $\Delta {\mathbf{\varphi }}_k$ is given by Equation (8) and the non-gravitational changes in the position $\Delta {\mathit{r}}_{ng}$ and velocity $\Delta v_{ng}$ are given by Equations (10) and (14). The incorporation of the CSS corrections accounts for the vector rotation between $t_{k-1}$ to $t_k$; this is also done in more traditional approaches [22,26], though the integration of a single set of IMU measurements only provides a first-order correction for the vector rotation. Savage [14] similarly posed these dynamics with the CSS corrections, though he instead considered a rotating navigation frame (such as the East–North–Up (ENU) frame) and implicitly assumed that the origin of the IMU case frame and body’s center of gravity were collocated.

2.5. Strapdown IMU Model

The discrete inertial measurements used for the standard SINS are a function of the true quantity being measured and several contributing measurement corruption parameters that result from a variety of sources such as manufacturing tolerances, sensor installation, and unit degradation. The measurement model considered for the strapdown IMU is described by

$$\begin{array}{cc}\hfill \Delta {\mathbf{\theta }}_{m,k}& =({\mathit{I}}_{3\times 3}+[{\mathit{s}}_{g,k}\setminus ])({\mathit{I}}_{3\times 3}+[{\mathit{m}}_{g,k}\times ]+[{\mathit{n}}_{g,k}\ast ])(\Delta {\mathbf{\theta }}_k+{\mathit{b}}_{g,k}+{\mathit{w}}_{g,k})\hfill \end{array}$$

(22a)

$$\begin{array}{cc}\hfill \Delta v_{m,k}& =({\mathit{I}}_{3\times 3}+[{\mathit{s}}_{a,k}\setminus ])({\mathit{I}}_{3\times 3}+[{\mathit{m}}_{a,k}\times ]+[{\mathit{n}}_{a,k}\ast ])(\Delta v_k+{\mathit{b}}_{a,k}+{\mathit{w}}_{a,k}),\hfill \end{array}$$

(22b)

where ${\mathit{b}}_{g,k}$, ${\mathit{s}}_{g,k}$, ${\mathit{n}}_{g,k}$, ${\mathit{m}}_{g,k}$, and ${\mathit{w}}_{g,k}$ are the bias, scale factor, nonorthogonality, misalignment, and zero-mean, time-wise uncorrelated noise in the gyroscope measurement at $t_k$, respectively, while $\Delta {\mathbf{\theta }}_k$ is the true incremental angle, and $\Delta {\mathbf{\theta }}_{m,k}$ is the measured incremental angle at $t_k$. Similarly, ${\mathit{b}}_{a,k}$, ${\mathit{s}}_{a,k}$, ${\mathit{n}}_{a,k}$, ${\mathit{m}}_{a,k}$, and ${\mathit{w}}_{a,k}$ are the bias, scale factor, nonorthogonality, misalignment, and zero-mean, time-wise uncorrelated noise in the accelerometer measurement at $t_k$, respectively, $\Delta v_k$ is the true incremental velocity, and $\Delta v_{m,k}$ is the measured incremental velocity at $t_k$. The operators used in Equation (22) are defined such that, for $\mathit{y}\in {\mathbb{R}}^3$,

$$\begin{array}{c}[\mathit{y}\setminus ]=\left[\begin{array}{ccc}{y}_1& 0& 0\\ 0& y_2& 0\\ 0& 0& y_3\end{array}\right]\mathrm{and}[\mathit{y}\ast ]=\left[\begin{array}{ccc}0& y_3& y_2\\ y_3& 0& y_1\\ y_2& y_1& 0\end{array}\right].\end{array}$$

Applying the models given in Equation (22), the true incremental angle and incremental velocity can be obtained from the measured quantities such that

$$\begin{array}{cc}\hfill \Delta {\mathbf{\theta }}_k& ={({\mathit{I}}_{3\times 3}+[{\mathit{m}}_{g,k}\times ]+[{\mathit{n}}_{g,k}\ast ])}^{-1}{({\mathit{I}}_{3\times 3}+[{\mathit{s}}_{g,k}\setminus ])}^{-1}\Delta {\mathbf{\theta }}_{m,k}-{\mathit{b}}_{g,k}-{\mathit{w}}_{g,k}\hfill \end{array}$$

(23a)

$$\begin{array}{cc}\hfill \Delta v_k& ={({\mathit{I}}_{3\times 3}+[{\mathit{m}}_{a,k}\times ]+[{\mathit{n}}_{a,k}\ast ])}^{-1}{({\mathit{I}}_{3\times 3}+[{\mathit{s}}_{a,k}\setminus ])}^{-1}\Delta v_{m,k}-{\mathit{b}}_{a,k}-{\mathit{w}}_{a,k}.\hfill \end{array}$$

(23b)

It can then be shown, through the application of the matrix inversion lemma [27], that Equation (23) can be approximated by

$$\begin{array}{cc}\hfill \Delta {\mathbf{\theta }}_k& =\Delta {\mathbf{\theta }}_{m,k}-[\Delta {\mathbf{\theta }}_{m,k}\setminus ]{\mathit{s}}_{g,k}+[\Delta {\mathbf{\theta }}_{m,k}\times ]{\mathit{m}}_{g,k}-[\Delta {\mathbf{\theta }}_{m,k}\ast ]{\mathit{n}}_{g,k}-{\mathit{b}}_{g,k}-{\mathit{w}}_{g,k}\hfill \end{array}$$

(24a)

$$\begin{array}{cc}\hfill \Delta v_k& =\Delta v_{m,k}-[\Delta v_{m,k}\setminus ]{\mathit{s}}_{a,k}+[\Delta v_{m,k}\times ]{\mathit{m}}_{a,k}-[\Delta v_{m,k}\ast ]{\mathit{n}}_{a,k}-{\mathit{b}}_{a,k}-{\mathit{w}}_{a,k}.\hfill \end{array}$$

(24b)

Often, statistics surrounding these parametric errors are studied and detailed by sensor manufacturers, though they can also be determined through independent testing and analysis. If the estimated incremental angle and velocity are denoted as $\Delta {\widehat{\mathbf{\theta }}}_i$ and $\Delta {\widehat{v}}_i$, respectively, then the error between the true and estimated incremental angle and velocity can be defined such that

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta \theta ,i}\triangleq \Delta {\mathbf{\theta }}_i-\Delta {\widehat{\mathbf{\theta }}}_i\mathrm{and}{\mathit{e}}_{\Delta v,i}\triangleq \Delta v_i-\Delta {\widehat{v}}_i.\end{array}$$

(25)

Fusing the available statistics with the expressions in Equations (24) and (25), a model of how errors in the sensor specific parameters propagate into the IMU measurement errors can be developed. Furthermore, expressions detailing the propagation of these parameteric errors into the state estimation error can be developed and used for implementation within the navigation system architecture.

3. Error Propagation Development

By performing the CSS corrections, a correction factor is determined for the raw measurements, allowing the results to better represent the true dynamics of the vehicle. However, when considering the propagation of errors through these corrections, it is clear that if the manufacturer-provided performance specifications for the sensor bias, noise, etc., are for the raw measurements at the sensor’s operational frequency, then these statistics will be inconsistent with the output when CSS corrections are introduced. To have an accurate uncertainty representation, the navigation filter’s covariance prediction requires the description of how errors propagate into the estimate to be representative of the system dynamics. In situations where the navigation system relies upon CSS correction algorithms, the CSS corrections aid in the dynamics description and thus have an influence on the error propagation. Therefore, a rigorous expression for the uncertainty in CSS corrections can be determined and implemented within common navigation system architectures by examining the algorithms and mapping the errors through each.

3.1. Method for Error Analysis

To determine how the sensor errors propagate through the CSS algorithms contained within Section 2, the transformation of measurement errors through each correction term is examined. To develop the transformation, it is necessary to consider the estimation error in a given variable to be $\mathit{e}=\mathit{x}-\widehat{\mathit{x}}$, such that the error, $\mathit{e}$, is expressed as the difference between the true, $\mathit{x}$, and estimated, $\widehat{\mathit{x}}$, quantities. Additionally, as seen in Equations (8), (10), and (14), the output of each algorithm can be expressed as a function of the measurement accumulation and the correction terms, while the error dynamics for covariance propagation must be expressed as a function of the estimate and the error in each quantity. To aid in this development, the result of each algorithm is broken into smaller components and recombined to develop the full error dynamics for a given correction.

For example, to determine the coned measurement error, it can be recast as functions of the accumulated incremental angle and coning correction errors such that

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta \varphi ,k}={\mathit{e}}_{\theta ,\ell }+{\mathit{e}}_{\beta ,\ell }.\end{array}$$

(26)

Noting that the coning correction is a function of the measurements, it is then possible to independently express ${\mathit{e}}_{\theta ,\ell }$ and ${\mathit{e}}_{\beta ,\ell }$ as functions of the measurement errors ${\mathit{e}}_{\Delta \theta ,i}\forall i\in \{1,2,\cdots \ell \}$ as

$$\begin{array}{c}\hfill {\mathit{e}}_{\theta ,\ell }=\mathit{f}\left({\mathit{e}}_{\Delta \theta ,i}\right)\mathrm{and}{\mathit{e}}_{\beta ,\ell }=\mathit{g}\left({\mathit{e}}_{\Delta \theta ,i}\right),\end{array}$$

(27)

where $\mathit{f}$ and $\mathit{g}$ are taken to be independent functions that describe the transformation of sensor errors into the accumulated incremental angle and coning correction vectors, respectively. Therefore, after developing the expressions that map the sensor errors into the accumulation and the correction, the error in the coned measurement can be written as a function of the sensor errors.

The same can be done for the sculled and scrolled non-gravitational changes in velocity and position, respectively. The error mapping is developed by first generating the mapping for each term in the correction individually and then combining the components to construct a mapping of the sensor error sources through the algorithm and into the resulting correction. After developing the mapping of the errors through the algorithms, a slight simplification can be made to each by assuming that some of the error sources are approximately constant over a single major interval. Finally, the transformation of the error into the state estimate can be written as a function of each contributing error source.

3.2. Parametric Estimation Errors

The uncertain parameters used to account for sensor errors in IMU measurements are typically identified as neglected, considered, or estimated quantities within the navigation filter, pending the results of a consider analysis as originally posed by S.F. Schmidt [28], though now widely used in application and research [29,30,31,32,33]. In the case in which all of the error parameters are estimated directly by the navigation filter, and assuming that Equation (24) holds, the expected values of the incremental angle $\mathbb{E}\{\Delta {\mathbf{\theta }}_k\}=\Delta {\widehat{\mathbf{\theta }}}_k$ and the incremental velocity $\mathbb{E}\{\Delta v_k\}=\Delta {\widehat{v}}_k$ are seen to be

$$\begin{array}{cc}\hfill \Delta {\widehat{\mathbf{\theta }}}_k& =\Delta {\mathbf{\theta }}_{m,k}-[\Delta {\mathbf{\theta }}_{m,k}\setminus ]{\widehat{\mathit{s}}}_{g,k}+[\Delta {\mathbf{\theta }}_{m,k}\times ]{\widehat{\mathit{m}}}_{g,k}-[\Delta {\mathbf{\theta }}_{m,k}\ast ]{\widehat{\mathit{n}}}_{g,k}-{\widehat{\mathit{b}}}_{g,k}\hfill \end{array}$$

(28a)

$$\begin{array}{cc}\hfill \Delta {\widehat{v}}_k& =\Delta v_{m,k}-[\Delta v_{m,k}\setminus ]{\widehat{\mathit{s}}}_{a,k}+[\Delta v_{m,k}\times ]{\widehat{\mathit{m}}}_{a,k}-[\Delta v_{m,k}\ast ]{\widehat{\mathit{n}}}_{a,k}-{\widehat{\mathit{b}}}_{a,k},\hfill \end{array}$$

(28b)

where ${\widehat{\mathit{b}}}_{g,k}$, ${\widehat{\mathit{s}}}_{g,k}$, ${\widehat{\mathit{n}}}_{g,k}$, and ${\widehat{\mathit{m}}}_{g,k}$ are the estimated or expected bias, scale factor, nonorthogonality, and misalignment in the gyroscope measurements, respectively, and ${\widehat{\mathit{b}}}_{a,k}$, ${\widehat{\mathit{s}}}_{a,k}$, ${\widehat{\mathit{n}}}_{a,k}$, and ${\widehat{\mathit{m}}}_{a,k}$ are the estimated or expected bias, scale factor, nonorthogonality, and misalignment in the accelerometer measurements, respectively. Note that the noise is defined to be zero-mean. By subtracting Equation (28) from Equation (24) and simplifying, the error in the measurements can be expressed as

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta \theta ,k}& =-[\Delta {\mathbf{\theta }}_{m,k}\setminus ]{\mathit{e}}_{s_g,k}+[\Delta {\mathbf{\theta }}_{m,k}\times ]{\mathit{e}}_{m_g,k}-[\Delta {\mathbf{\theta }}_{m,k}\ast ]{\mathit{e}}_{n_g,k}-{\mathit{e}}_{b_g,k}-{\mathit{w}}_{g,k}\hfill \end{array}$$

(29a)

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta v,k}& =-[\Delta v_{m,k}\setminus ]{\mathit{e}}_{s_a,k}+[\Delta v_{m,k}\times ]{\mathit{e}}_{m_a,k}-[\Delta v_{m,k}\ast ]{\mathit{e}}_{n_a,k}-{\mathit{e}}_{b_a,k}-{\mathit{w}}_{a,k},\hfill \end{array}$$

(29b)

where the errors in the bias, scale factor, misalignment, and nonorthogonality estimates for the gyroscopes are defined to be

$$\begin{array}{c}{\mathit{e}}_{b_g,k}\triangleq {\mathit{b}}_{g,k}-{\widehat{\mathit{b}}}_{g,k},{\mathit{e}}_{s_g,k}\triangleq {\mathit{s}}_{g,k}-{\widehat{\mathit{s}}}_{g,k},{\mathit{e}}_{m_g,k}\triangleq {\mathit{m}}_{g,k}-{\widehat{\mathit{m}}}_{g,k},\mathrm{and}{\mathit{e}}_{n_g,k}\triangleq {\mathit{n}}_{g,k}-{\widehat{\mathit{n}}}_{g,k},\end{array}$$

respectively, and, similarly, the errors in the bias, scale factor, misalignment, and nonorthogonality estimates for the accelerometers are

$$\begin{array}{c}{\mathit{e}}_{b_a,k}\triangleq {\mathit{b}}_{a,k}-{\widehat{\mathit{b}}}_{a,k},{\mathit{e}}_{s_a,k}\triangleq {\mathit{s}}_{a,k}-{\widehat{\mathit{s}}}_{a,k},{\mathit{e}}_{m_a,k}\triangleq {\mathit{m}}_{a,k}-{\widehat{\mathit{m}}}_{a,k},\mathrm{and}{\mathit{e}}_{n_a,k}\triangleq {\mathit{n}}_{a,k}-{\widehat{\mathit{n}}}_{a,k},\end{array}$$

respectively. Through this model, sensor errors can be attributed to and expressed as a function of parametric estimation errors.

3.3. Coning Error Propagation

Consider the error in the incremental angle internal measurements to be expressed as

$$\begin{array}{c}\hfill {\mathit{e}}_{\theta ,\ell }\triangleq {\mathbf{\theta }}_{\ell }-{\widehat{\mathbf{\theta }}}_{\ell },\end{array}$$

(30)

where ${\mathbf{\theta }}_{\ell }$ and ${\widehat{\mathbf{\theta }}}_{\ell }$ are the true and estimated accumulated rotation vectors for the major interval, respectively. An expression describing the propagation of errors through the coning algorithm presented in Section 2.1 can be shown to be [34].

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta \varphi }=\sum _{i=1}^{\ell }{\mathit{e}}_{\Delta \theta ,i}+{\mathsf{\Xi }}_{con,i}{\mathit{e}}_{\Delta \theta ,i}=\sum _{i=1}^{\ell }\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}\right){\mathit{e}}_{\Delta \theta ,i},\end{array}$$

(31)

where ${\mathsf{\Xi }}_{con,i}\triangleq [{\mathbf{\xi }}_{con,i}\times ]$ and

$$\begin{array}{c}\hfill {\mathbf{\xi }}_{con,i}=\frac{1}{2}\left[\sum _{j=1}^{i-1}\Delta {\widehat{\mathbf{\theta }}}_j-\sum _{j=i+1}^{\ell }\Delta {\widehat{\mathbf{\theta }}}_j\right]-\frac{1}{12}\left(\Delta {\widehat{\mathbf{\theta }}}_{i+1}-\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right).\end{array}$$

(32)

With Equation (32), it can also be shown that the error in the i^th coning correction term is not correlated to the i^th measurement, but only to those prior to and following its processing. Additionally, if $i+1\ge \ell$ or $i-1\le 0$, then $\Delta {\widehat{\mathbf{\theta }}}_{i+1}=\mathbf{0}$ or $\Delta {\widehat{\mathbf{\theta }}}_{i-1}=\mathbf{0}$, respectively. To generate ${\mathbf{\xi }}_{con,i}$ as stated in Equation (32), the entire array of ℓ measurements must be known; fortunately, this can be restated so that the error terms can be accumulated in a navigation preprocessor algorithm, much like the coning algorithm itself.

3.4. Sculling Error Propagation

Through the application of a sculling algorithm, a correction for the measured non-gravitational specific force and its integration into the vehicle’s velocity is made using the incremental angle and velocity measurements over the major interval. It is worth noting that the statistics surrounding the sculling corrected incremental velocity will be dependent upon both the incremental angle and incremental velocity, unlike the coning correction. Noting the equation governing the sculling-corrected non-gravitational term in Equation (10), the error can be written as a sum of errors in each of the components; i.e.,

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{ng,\ell }}={\mathit{e}}_{v,\ell }+{\mathit{e}}_{\Delta v_{scul},\ell }+{\mathit{e}}_{\Delta v_{rot},\ell },\end{array}$$

(33)

where ${\mathit{e}}_{v,\ell }$ is the error in the incremental angle accumulation, ${\mathit{e}}_{\Delta v_{scul},\ell }$ is the error in the sculling correction, and ${\mathit{e}}_{\Delta v_{rot},\ell }$ is the error in correction for the vehicle’s rotation during the measurement accumulation.

The error in the accumulated incremental velocity can be expressed as

$$\begin{array}{c}\hfill {\mathit{e}}_{v,\ell }\triangleq v_{\ell }-{\widehat{v}}_{\ell },\end{array}$$

(34)

where $v_{\ell }$ and ${\widehat{v}}_{\ell }$ are the true and estimated accumulated velocity vector over the major interval, respectively. By the definition of the incremental velocity accumulation in Equation (9) and the definition of the incremental angle accumulation in Equation (6), it is clear from Equation (25) that the error in each accumulated error is expressed as a sum over all minor interval incremental velocity and angle errors such that

$$\begin{array}{c}\hfill {\mathit{e}}_{v,\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\Delta v,i}\mathrm{and}{\mathit{e}}_{\theta ,\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\Delta \theta ,i}.\end{array}$$

(35)

To determine the error in the sculling correction, first recognize that the sculling correction is a sum of the incremental sculling corrections, as shown in Equation (11), where the increments are defined by Equation (12). Therefore, to determine the error in the sculling correction, the error in the increments must first be determined. Define the error in the sculling increment ${\mathit{e}}_{\delta v_{scul},i}$ such that

$$\begin{array}{c}\hfill {\mathit{e}}_{\delta v_{scul},i}\triangleq \delta v_{scul,i}-\delta {\widehat{v}}_{scul,i}.\end{array}$$

(36)

Applying the definition of the individual measurement errors and the accumulations, the propagation of errors through the sculling correction is approximated to first-order as

$$\begin{array}{cc}\hfill {\mathit{e}}_{\delta v_{scul},i}=& \left({\widehat{\mathbf{\theta }}}_{i-1}+\frac{1}{6}\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)\times {\mathit{e}}_{\Delta v,i}-\Delta {\widehat{\mathbf{\theta }}}_i\times \left({\mathit{e}}_{v,i-1}+\frac{1}{6}{\mathit{e}}_{\Delta v,i-1}\right)\hfill \\ \hfill & +\left({\widehat{v}}_{i-1}+\frac{1}{6}\Delta {\widehat{v}}_{i-1}\right)\times {\mathit{e}}_{\Delta \theta ,i}-\Delta {\widehat{v}}_i\times \left({\mathit{e}}_{\theta ,i-1}+\frac{1}{6}{\mathit{e}}_{\Delta \theta ,i-1}\right).\hfill \end{array}$$

(37)

By the definition of the accumulated incremental sculling corrections given in Equation (11), the accumulated error is simply a sum of the incremental errors; i.e.,

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{scul},\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\delta v_{scul},i}.\end{array}$$

(38)

Now that an expression for the error in the accumulated sculling correction is known, the explicit mapping of the error in each measurement into the accumulated error is desired. Notice that Equation (37) has two components that parallel the form of the propagation of the incremental angle measurements through the coning correction—the first is the previously defined ${\mathsf{\Xi }}_{con,i}$, crossed with the incremental velocity errors, while the second term is a similar mapping that can be defined such that ${\mathsf{\Xi }}_{scul,i}\triangleq [{\mathbf{\xi }}_{scul,i}\times ]$, where

$$\begin{array}{c}\hfill {\mathbf{\xi }}_{scul,i}=\frac{1}{2}\left[\sum _{j=1}^{i-1}\Delta {\widehat{v}}_j-\sum _{j=i+1}^{\ell }\Delta {\widehat{v}}_j\right]-\frac{1}{12}\left(\Delta {\widehat{v}}_{i+1}-\Delta {\widehat{v}}_{i-1}\right),\end{array}$$

(39)

and further allows the definition of the mapping of the measurement errors through the incremental sculling correction to be

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{scul},\ell }=\sum _{i=1}^{\ell }\left({\mathsf{\Xi }}_{con,i}{\mathit{e}}_{\Delta v,i}+{\mathsf{\Xi }}_{scul,i}{\mathit{e}}_{\Delta \theta ,i}\right).\end{array}$$

(40)

The direction of the incremental velocity vector must compensate for the vehicle’s rotation during the major interval; this is done via the rotational correction term. To determine the mapping of the measurement errors through the rotational correction term, define the error in the rotational correction as

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{rot},\ell }\triangleq \Delta v_{rot,\ell }-\Delta {\widehat{v}}_{rot,\ell }.\end{array}$$

(41)

Expanding Equation (41) with the definition of $\Delta v_{rot,\ell }$ in Equation (13) and simplifying, the error in the rotational correction is then

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta v_{rot},\ell }& =\frac{1}{2}\left[{\widehat{\mathbf{\theta }}}_{\ell }\times {\mathit{e}}_{v,\ell }-{\widehat{v}}_{\ell }\times {\mathit{e}}_{\theta ,\ell }\right],\hfill \end{array}$$

(42)

when higher-order error terms are neglected. Given that the errors in the accumulations are simply a sum of errors in each of the measurements, the error propagation for the rotational correction is

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{rot},\ell }=\frac{1}{2}\sum _{i=1}^{\ell }\left(\left[{\widehat{\mathbf{\theta }}}_{\ell }\times \right]{\mathit{e}}_{\Delta v,i}-\left[{\widehat{v}}_{\ell }\times \right]{\mathit{e}}_{\Delta \theta ,i}\right).\end{array}$$

(43)

To produce the error propagation for the sculled, non-gravitational change in velocity, Equation (33) is combined with the definitions for each component defined in Equations (35), (40), and (43). Therefore, the error in the sculling term as a function of the estimated incremental angles and velocities mapped into the errors in each of those terms is

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta v_{ng},\ell }=\sum _{i=1}^{\ell }\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}+\frac{1}{2}\left[{\widehat{\mathbf{\theta }}}_{\ell }\times \right]\right){\mathit{e}}_{\Delta v,i}+\sum _{i=1}^{\ell }\left({\mathsf{\Xi }}_{scul,i}-\frac{1}{2}\left[{\widehat{v}}_{\ell }\times \right]\right){\mathit{e}}_{\Delta \theta ,i}.\end{array}$$

(44)

The errors in each of these measurement sources, however, is a function of well-known and commonly estimated error sources.

3.5. Scrolling Error Propagation

Paralleling the results in Section 3.4, a correction to the measured non-gravitational and its integration into position is made using the incremental angle and velocity measurements by the application of the scrolling algorithm. Whereas the coning correction is dependent upon the statistics surrounding the incremental angle measurements, the scrolling corrections are more similar to the sculling corrections as they are also dependent upon the statistics for the incremental velocity. Noting that the equation governing the scrolling-corrected non-gravitational term is Equation (14), the error can be written as a sum of errors in each of the components; i.e.,

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta r_{ng},k}={\mathit{e}}_{s_v,\ell }+{\mathit{e}}_{\Delta r_{rot},\ell }+{\mathit{e}}_{\Delta r_{scrl},\ell },\end{array}$$

(45)

where ${\mathit{e}}_{s_v,\ell }$ is the error in the incremental angle accumulation, ${\mathit{e}}_{\Delta r_{scrl},\ell }$ is the error in the scrolling correction, and ${\mathit{e}}_{\Delta r_{rot},\ell }$ is the error in the correction for the vehicle’s rotation during the measurement accumulation. Each of these terms is examined separately and recombined to produce the desired result.

The first term in Equation (45) describes the error introduced through the integration of the incremental velocity vectors to determine the change in the vehicle’s position, with the increment and accumulation defined in Equations (15) and (16), respectively. The error must then be expressed in terms of the error in the accumulation and increment; the error in the accumulation is simply a sum of the error in each increment, i.e.,

$$\begin{array}{c}\hfill {\mathit{e}}_{s_v,\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\Delta s_v,i}.\end{array}$$

(46)

The error in the increment is then defined to be the difference between the estimated and true increments, which is given by ${\mathit{e}}_{\Delta s_v,i}=\Delta {\mathit{s}}_{v,i}-\Delta {\widehat{\mathit{s}}}_{v,i}$. Substituting for the definition of the increment and truth, the error in the increment can be simplified and expressed as

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta s_v,i}& ={\mathit{e}}_{v,i-1}\Delta t_{\ell }+\frac{1}{12}\left(5{\mathit{e}}_{\Delta v,i}+{\mathit{e}}_{\Delta v,i-1}\right)\Delta t_{\ell },\hfill \end{array}$$

(47)

allowing Equation (46) to be expressed as a sum of the errors in each increment. Note that the ${\mathit{e}}_{v,i-1}$ term has been deconstructed and expressed as a sum of the increments by applying Equation (35). Expanding for a variable number of steps, the propagation of incremental velocity errors into the accumulated integrated velocity is given by

$$\begin{array}{c}\hfill {\mathit{e}}_{s_v,\ell }=\Delta t_{\ell }\sum _{i=1}^{\ell }{c}_{i,\ell }{\mathit{e}}_{\Delta v,i},\end{array}$$

(48)

with the coefficient $c_{m,n}$ defined to be

$$\begin{array}{c}\hfill c_{i,j}=\left\{\begin{array}{cc}\frac{1}{2}+n-m\hfill & m<n\hfill \\ \frac{5}{12}\hfill & m=n,\hfill \end{array}\right.\end{array}$$

(49)

where m denotes the index of the measurement error term with which the coefficient is associated, and n is the number of measurements contained within the ${\mathit{e}}_{s_v,\ell }$ term. In most cases, $m=\ell$, though this is not always true.

To accurately propagate the vehicle states with the non-gravitational incremental velocity and angle, the rotation of the vectors during the measurement period is accounted for and defined in Equation (17). When considering uncertainty, however, the transformation of errors in each of the measurements to the correction must be determined. Define the error in the rotational component to be

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta r_{rot,k}}\triangleq \Delta {\mathit{r}}_{rot,k}-\Delta {\widehat{\mathit{r}}}_{rot,k},\end{array}$$

(50)

and the error in the integrated incremental angle ${\mathit{e}}_{s_{\theta },\ell }$ can be expressed similarly to how the incremental velocity is defined in Equation (48) as

$$\begin{array}{c}\hfill {\mathit{e}}_{s_{\theta },\ell }=\Delta t_{\ell }\sum _{i=1}^{\ell }{c}_{i,\ell }{\mathit{e}}_{\Delta {\theta }_i},\end{array}$$

(51)

with $c_{i,\ell }$ defined by Equation (49), where $m=i$ and $n=\ell$. It can then be shown that the error in the rotational scrolling term can be expressed as a function of the measurement errors as

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta r_{rot,k}}=\frac{1}{6}\sum _{i=1}^{\ell }\left({\mathsf{\Lambda }}_{\Delta \theta ,i}{\mathit{e}}_{\Delta v_i}-{\mathsf{\Lambda }}_{\Delta v,i}{\mathit{e}}_{\Delta {\theta }_i}\right),\end{array}$$

(52)

where ${\mathsf{\Lambda }}_{\Delta \theta ,i}\triangleq \left[\mathit{\lambda }\Delta \theta ,i\times \right]$, ${\mathsf{\Lambda }}_{\Delta v,i}\triangleq \left[\mathit{\lambda }\Delta v,i\times \right]$, and

$$\begin{array}{cc}\hfill \mathit{\lambda }\Delta \theta ,i& ={\widehat{\mathit{s}}}_{\theta ,k}+c_{i,\ell }\Delta t_{\ell }{\widehat{\mathbf{\theta }}}_k\hfill \end{array}$$

(53a)

$$\begin{array}{cc}\hfill \mathit{\lambda }\Delta v,i& ={\widehat{\mathit{s}}}_{v,k}+c_{i,\ell }\Delta t_{\ell }{\widehat{v}}_k.\hfill \end{array}$$

(53b)

With ${\mathsf{\Lambda }}_{\Delta \theta ,i}$ and ${\mathsf{\Lambda }}_{\Delta v,i}$ defined, the mapping of measurement errors through the position rotational correction is known.

The scrolling correction term, as described by Equation (18), is composed of an accumulation of two incremental corrections: a correction for the presence of sculling motion and its integration into the position and a correction for the presence of other, higher-order effects. The error in the accumulation of the scrolling correction can be expressed as

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta r_{scrl},\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\delta r_{scrl/scul},i}+\sum _{i=1}^{\ell }{\mathit{e}}_{\delta r_{scrl/other},i},\end{array}$$

(54)

where the error in each increment is defined to be

$$\begin{array}{cc}\hfill {\mathit{e}}_{\delta r_{scrl/scul},i}& \triangleq \delta {\mathit{r}}_{scrl/scul,i}-\delta {\widehat{\mathit{r}}}_{scrl/scul,i}\hfill \end{array}$$

(55a)

$$\begin{array}{cc}\hfill {\mathit{e}}_{\delta r_{scrl/other},i}& \triangleq \delta {\mathit{r}}_{scrl/other,i}-\delta {\widehat{\mathit{r}}}_{scrl/other,i}.\hfill \end{array}$$

(55b)

Applying the definition in Equation (19) to Equation (55a), the propagation of sensor errors through the incremental scrolling correction for sculling motion is determined to be

$$\begin{array}{cc}\hfill {\mathit{e}}_{\delta r_{scrl/scul},i}& =\Delta t_{\ell }\sum _{j=1}^{i-1}\left({\mathsf{\Xi }}_{con,j}{\mathit{e}}_{\Delta v,j}+{\mathsf{\Xi }}_{scul,j}{\mathit{e}}_{\Delta \theta ,j}\right)\hfill \\ \hfill & -\frac{\Delta t_{\ell }}{24}\left(5\Delta {\widehat{\mathbf{\theta }}}_i+\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)\times {\mathit{e}}_{v,i-1}-\frac{\Delta t_{\ell }}{24}\left(5\Delta {\widehat{v}}_i+\Delta {\widehat{v}}_{i-1}\right)\times {\mathit{e}}_{\theta ,i-1}\hfill \\ \hfill & +\frac{\Delta t_{\ell }}{24}\left({\widehat{\mathbf{\theta }}}_{i-1}-\frac{1}{2}\Delta {\widehat{\mathbf{\theta }}}_i\right)\times {\mathit{e}}_{\Delta v,i-1}+\frac{\Delta t_{\ell }}{24}\left({\widehat{v}}_{i-1}-\frac{1}{2}\Delta {\widehat{v}}_i\right)\times {\mathit{e}}_{\Delta \theta ,i-1}\hfill \\ \hfill & +\frac{\Delta t_{\ell }}{24}\left(5{\widehat{\mathbf{\theta }}}_{i-1}+\frac{1}{2}\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)\times {\mathit{e}}_{\Delta v,i}+\frac{\Delta t_{\ell }}{24}\left(5{\widehat{v}}_{i-1}+\frac{1}{2}\Delta {\widehat{v}}_{i-1}\right)\times {\mathit{e}}_{\Delta \theta ,i}.\hfill \end{array}$$

(56)

It is worth noting the dependency of Equation (56) on the sculling correction, as expected. For brevity, the derivation of Equation (56) is neglected here but can be found in [35].

Using the expression describing the propagation of measurement errors through the incremental correction, the propagation of the measurement errors through the scrolling algorithm’s correction for sculling motion is simply a sum of errors in each increment generated across the major interval. Expanding manually, it can be shown that the error in the incremental scrolling correction for sculling motion is then expressed as

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta r_{scrl/scul},\ell }& =\Delta t_{\ell }[\sum _{i=1}^{\ell }\left(\left(\ell -i\right){\mathsf{\Xi }}_{con,i}+\frac{1}{24}{\mathsf{\Gamma }}_{\Delta \theta ,i}\right){\mathit{e}}_{\Delta v,i}\hfill \\ \hfill & +\sum _{i=1}^{\ell }\left(\left(\ell -i\right){\mathsf{\Xi }}_{scul,i}+\frac{1}{24}{\mathsf{\Gamma }}_{\Delta v,i}\right){\mathit{e}}_{\Delta \theta ,i}],\hfill \end{array}$$

(57)

with the mappings defined such that ${\mathsf{\Gamma }}_{\Delta \theta ,i}\triangleq \left[{\mathbf{\gamma }}_{\Delta \theta ,i}\times \right]$ and ${\mathsf{\Gamma }}_{\Delta v,i}\triangleq \left[{\mathbf{\gamma }}_{\Delta v,i}\times \right]$, where

$$\begin{array}{c}\hfill {\mathbf{\gamma }}_{\Delta \theta ,i}=\left\{\begin{array}{cc}6\left(\sum _{j=1}^{i-1}\Delta {\widehat{\mathbf{\theta }}}_j-\sum _{j=i+1}^{\ell }\Delta {\widehat{\mathbf{\theta }}}_j\right)+\frac{1}{2}\left(\Delta {\widehat{\mathbf{\theta }}}_{i-1}-\Delta {\widehat{\mathbf{\theta }}}_{i+1}\right)+\Delta {\widehat{\mathbf{\theta }}}_{\ell }\hfill & i<\ell \hfill \\ 5\sum _{j=1}^{i-1}\Delta {\widehat{\mathbf{\theta }}}_j+\frac{1}{2}\Delta {\widehat{\mathbf{\theta }}}_{i-1}\hfill & i=\ell \hfill \end{array}\right.\end{array}$$

and

$$\begin{array}{c}\hfill {\mathbf{\gamma }}_{\Delta v,i}=\left\{\begin{array}{cc}6\left(\sum _{j=1}^{i-1}\Delta {\widehat{v}}_j-\sum _{j=i+1}^{\ell }\Delta {\widehat{v}}_j\right)+\frac{1}{2}\left(\Delta {\widehat{v}}_{i-1}-\Delta {\widehat{v}}_{i+1}\right)+\Delta {\widehat{v}}_{\ell }\hfill & i<\ell \hfill \\ 5\sum _{j=1}^{i-1}\Delta {\widehat{v}}_j+\frac{1}{2}\Delta {\widehat{v}}_{i-1}\hfill & i=\ell .\hfill \end{array}\right.\end{array}$$

The scrolling correction error introduced by the additional correction for higher-order effects is a sum of the error in each incremental correction made over the major interval. In any given increment, the propagation of each measurement error into the scrolling correction increment for higher-order effects can be defined such that [35]

$$\begin{array}{cc}\hfill {\mathit{e}}_{\delta r_{scrl/other},i}=& \frac{\Delta t_{\ell }}{6}\sum _{j=1}^{i-1}\left(\left[{\widehat{\mathbf{\theta }}}_{i-1}-\frac{1}{6}\left(\Delta {\widehat{\mathbf{\theta }}}_i-\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)-c_{j,i-1}\Delta {\widehat{\mathbf{\theta }}}_i\right]\times {\mathit{e}}_{\Delta v,j}\right)\hfill \\ \hfill & -\frac{\Delta t_{\ell }}{6}\sum _{j=1}^{i-1}\left(\left[{\widehat{v}}_{i-1}-\frac{1}{6}\left(\Delta {\widehat{v}}_i-\Delta {\widehat{v}}_{i-1}\right)-c_{j,i-1}\Delta {\widehat{v}}_i\right]\times {\mathit{e}}_{\Delta \theta ,j}\right)\hfill \\ \hfill & +\frac{\Delta t_{\ell }}{6}\left[\frac{1}{6}{\widehat{\mathbf{\theta }}}_{i-1}-\frac{1}{40}\left(\Delta {\widehat{\mathbf{\theta }}}_i-\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)+\Delta {\widehat{\mathbf{\theta }}}_i\right]\times {\mathit{e}}_{\Delta v,i-1}\hfill \\ \hfill & -\frac{\Delta t_{\ell }}{6}\left[\frac{1}{6}{\widehat{v}}_{i-1}-\frac{1}{40}\left(\Delta {\widehat{v}}_i-\Delta {\widehat{v}}_{i-1}\right)+\Delta {\widehat{v}}_i\right]\times {\mathit{e}}_{\Delta \theta ,i-1}\hfill \\ \hfill & +\frac{\Delta t_{\ell }}{6}\left[\frac{7}{120}\left(\Delta {\widehat{\mathbf{\theta }}}_i-\Delta {\widehat{\mathbf{\theta }}}_{i-1}\right)-\frac{1}{24}\Delta {\widehat{\mathbf{\theta }}}_{i-1}+\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{\mathbf{\theta }}}_j\right]\times {\mathit{e}}_{\Delta v,i}\hfill \\ \hfill & -\frac{\Delta t_{\ell }}{6}\left[\frac{7}{120}\left(\Delta {\widehat{v}}_i-\Delta {\widehat{v}}_{i-1}\right)-\frac{1}{24}\Delta {\widehat{v}}_{i-1}+\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{v}}_j\right]\times {\mathit{e}}_{\Delta \theta ,i},\hfill \end{array}$$

(58)

where the coefficient $c_{j,i-1}$ is defined in Equation (49), with $m=j$ and $n=i-1$. The propagation of errors into the accumulated correction is then the sum of contributions for a given measurement error into each of the increments. Through a manual expansion of the terms, an expression mapping measurement errors into the scrolling correction term that accounts for higher-order effects is expressed as

$$\begin{array}{c}\hfill {\mathit{e}}_{\Delta r_{scrl/other},\ell }=\sum _{i=1}^{\ell }{\mathit{e}}_{\delta r_{scrl/other},i}=\frac{\Delta t_{\ell }}{6}\sum _{i=1}^{\ell }\left({\mathit{M}}_{\Delta v,i}{\mathit{e}}_{\Delta \theta ,i}-{\mathit{M}}_{\Delta \theta ,i}{\mathit{e}}_{\Delta v,i}\right),\end{array}$$

(59)

with ${\mathit{M}}_{\Delta \theta ,i}\triangleq \left[{\mathbf{\mu }}_{\Delta \theta ,i}\times \right]$ and ${\mathit{M}}_{\Delta v,i}\triangleq \left[{\mathbf{\mu }}_{\Delta v,i}\times \right]$, where ${\mathbf{\mu }}_{\Delta \theta ,i}$ and ${\mathbf{\mu }}_{\Delta v,i}$ can be expressed as

$$\begin{array}{c}\hfill {\mathbf{\mu }}_{\Delta \theta ,i}=\left\{\begin{array}{cc}\left(\ell -i+\frac{1}{6}\right){\widehat{\mathbf{\theta }}}_i-\frac{1}{10}\Delta {\widehat{\mathbf{\theta }}}_{i-1}+\frac{5}{4}\Delta {\widehat{\mathbf{\theta }}}_i+\frac{13}{120}\Delta {\widehat{\mathbf{\theta }}}_{i+1}-\frac{1}{6}\Delta {\widehat{\mathbf{\theta }}}_{\ell }\hfill & i<\ell \hfill \\ +\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{\mathbf{\theta }}}_j+\sum _{j=i+1}^{\ell }\left(\ell +i-2j+\frac{1}{2}\right)\Delta {\widehat{\mathbf{\theta }}}_j\hfill & \\ \hfill & \\ \frac{7}{120}\Delta {\widehat{\mathbf{\theta }}}_i-\frac{1}{10}\Delta {\widehat{\mathbf{\theta }}}_{i-1}+\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{\mathbf{\theta }}}_j\hfill & i=\ell \hfill \end{array}\right.\end{array}$$

(60)

and

$$\begin{array}{c}\hfill {\mathbf{\mu }}_{\Delta v,i}=\left\{\begin{array}{cc}\left(\ell -i+\frac{1}{6}\right){\widehat{v}}_i-\frac{1}{10}\Delta {\widehat{v}}_{i-1}+\frac{5}{4}\Delta {\widehat{v}}_i+\frac{13}{120}\Delta {\widehat{v}}_{i+1}-\frac{1}{6}\Delta {\widehat{v}}_{\ell }\hfill & i<\ell \hfill \\ +\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{v}}_j+\sum _{j=i+1}^{\ell }\left(\ell +i-2j+\frac{1}{2}\right)\Delta {\widehat{v}}_j\hfill & \\ \hfill & \\ \frac{7}{120}\Delta {\widehat{v}}_i-\frac{1}{10}\Delta {\widehat{v}}_{i-1}+\sum _{j=1}^{i-1}\left(i-j-\frac{1}{3}\right)\Delta {\widehat{v}}_j\hfill & i=\ell .\hfill \end{array}\right.\end{array}$$

(61)

The error in the scrolling-corrected non-gravitational change in position can be expressed by substituting the components from Equations (48), (52), (57), and (59), the propagation of errors is expressed as

$$\begin{array}{cc}\hfill {\mathit{e}}_{\Delta r_{ng,k}}=& \sum _{i=1}^{\ell }\left({\mathit{X}}_{\Delta v,i}{\mathit{e}}_{\Delta {\theta }_i}+{\mathit{X}}_{\Delta \theta ,i}{\mathit{e}}_{\Delta v_i}\right),\hfill \end{array}$$

(62)

where

$$\begin{array}{cc}\hfill {\mathit{X}}_{\Delta v,i}=& \Delta t_{\ell }\left((\ell -i){\mathsf{\Xi }}_{scul,i}+\frac{1}{24}{\mathsf{\Gamma }}_{\Delta v,i}+\frac{1}{6}{\mathit{M}}_{\Delta v,i}\right)-\frac{1}{6}{\mathsf{\Lambda }}_{\Delta v,i}\hfill \\ \hfill {\mathit{X}}_{\Delta \theta ,i}=& \Delta t_{\ell }\left(c_{i,\ell }{\mathit{I}}_{3\times 3}+(\ell -i){\mathsf{\Xi }}_{con,i}+\frac{1}{24}{\mathsf{\Gamma }}_{\Delta \theta ,i}-\frac{1}{6}{\mathit{M}}_{\Delta \theta ,i}\right)+\frac{1}{6}{\mathsf{\Lambda }}_{\Delta \theta ,i}\hfill \end{array}$$

are defined to simplify notation.

3.6. State Estimation Error Dynamics

For implementation within the multiplicative extended Kalman filter (MEKF) [36], a widely used tool developed to appropriately handle the quaternion representation of attitude, the transformation of errors through each of the CSS algorithms must be incorporated into the relationships describing the propagation of state estimation errors, where errors in the position and velocity states are defined to be

$$\begin{array}{c}\hfill {\mathit{e}}_{r,k}\triangleq {\mathit{r}}_{c,k}^i-{\widehat{\mathit{r}}}_{c,k}^i\mathrm{and}{\mathit{e}}_{v,k}\triangleq v_{c,k}^i-{\widehat{v}}_{c,k}^i,\end{array}$$

respectively. Unlike position and velocity, the attitude quaternions may not be directly subtracted. Thus, the attitude error is instead defined by the multiplicative attitude error; i.e.,

$$\begin{array}{c}\hfill \delta {\overline{\mathit{q}}}_{i,k}^c\triangleq {\overline{\mathit{q}}}_{i,k}^c\otimes {\left({\widehat{\overline{\mathit{q}}}}_{i,k}^c\right)}^{-1},\end{array}$$

(63)

as is necessary according to the MEKF architecture. Under a small angle assumption, the error in the attitude is approximately given by

$$\begin{array}{c}\hfill {\mathit{e}}_{a,k}=2\delta {\mathit{q}}_{i,k}^c,\end{array}$$

where $\delta {\mathit{q}}_{i,k}^c$ is the vector part of the error quaternion.

Examining the discrete-time dynamics in Equation (21), it can be shown that the error equations for the position, velocity, and attitude of the vehicle are given by [35]

$$\begin{array}{cc}\hfill {\mathit{e}}_{r,k}=& \left\{{\mathit{I}}_{3\times 3}+\frac{\Delta t_k^2}{2}\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right)\right\}{\mathit{e}}_{r,k-1}+\Delta t_k{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k^2}{6}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathit{r}}}_{ng,k}\times \right]\hfill \\ \hfill & -\frac{\Delta t_k^2}{2}\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\frac{\Delta t_k^2}{2}\left\{\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{3}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{d,k-1}\hfill \\ \hfill & +{\widehat{\mathit{T}}}_{k-1}^T{\mathit{e}}_{\Delta r_{ng},k}-\frac{\Delta t_k^2}{6}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{e}}_{\Delta \varphi ,k}\hfill \\ \hfill {\mathit{e}}_{v,k}=& \Delta t_k\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\mathit{e}}_{r,k-1}+{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{v}}_{ng,k}\times \right]\hfill \\ \hfill & -\Delta t_k\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\Delta t_k\left\{\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{d,k-1}\hfill \\ \hfill & +{\widehat{\mathit{T}}}_{k-1}^T{\mathit{e}}_{\Delta v_{ng},k}-\frac{\Delta t_k}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{e}}_{\Delta \varphi ,k}\hfill \\ \hfill {\mathit{e}}_{a,k}=& \mathit{T}(\Delta {\widehat{\mathbf{\varphi }}}_k){\mathit{e}}_{a,k-1}+{\mathit{e}}_{\Delta \varphi ,k}\hfill \end{array}$$

This result is similar to that of more traditional methods where only a single set of IMU measurements is used for propagation, as is seen in [26], though the contribution from the incremental angle and velocity are removed and housed within the ${\mathit{e}}_{\Delta v_{ng},k}$ and ${\mathit{e}}_{\Delta r_{ng},k}$ terms. Having previously described the transformation of errors through the CSS algorithms in Equations (31), (44), and (62), the state estimation error dynamics can then be expressed as

$$\begin{array}{cc}\hfill {\mathit{e}}_{r,k}& =\left\{{\mathit{I}}_{3\times 3}+\frac{\Delta t_k^2}{2}\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right)\right\}{\mathit{e}}_{r,k-1}+\Delta t_k{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k^2}{6}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathit{r}}}_{ng,k}\times \right]\hfill \\ \hfill & -\frac{\Delta t_k^2}{2}\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\frac{\Delta t_k^2}{2}\left\{\left({\widehat{g}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{3}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{r_{cg/c},k-1}\hfill \\ \hfill & +\sum _{i=1}^{\ell }\left\{{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}-\frac{\Delta t_k^2}{6}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}\right)\right\}{\mathit{e}}_{\Delta \theta ,i}\hfill \\ \hfill & +\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}{\mathit{e}}_{\Delta v,i}\hfill \end{array}$$

(64a)

$$\begin{array}{cc}\hfill {\mathit{e}}_{v,k}& =\Delta t_k\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\mathit{e}}_{r,k-1}+{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{v}}_{ng,k}\times \right]\hfill \\ \hfill & -\Delta t_k\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\Delta t_k\left\{\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{d,k-1}\hfill \\ \hfill & +\sum _{i=1}^{\ell }\left\{{\widehat{\mathit{T}}}_{k-1}^T\left({\mathsf{\Xi }}_{scul,i}-\frac{1}{2}\left[{\widehat{v}}_{\ell }\times \right]\right)-\frac{\Delta t_k}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}\right)\right\}{\mathit{e}}_{\Delta \theta ,i}\hfill \\ \hfill & +\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}+\frac{1}{2}\left[{\widehat{\mathbf{\theta }}}_{\ell }\times \right]\right){\mathit{e}}_{\Delta v,i}\hfill \end{array}$$

(64b)

$$\begin{array}{l}{\mathit{e}}_{a,k}=\mathit{T}(\Delta {\widehat{\mathbf{\varphi }}}_k){\mathit{e}}_{a,k-1}+{\mathit{e}}_{\Delta \varphi ,k}.\end{array}$$

(64c)

Note that ${\mathit{e}}_{d,k}$ is the error in the position of the vehicle center of gravity with respect to the IMU case frame origin, and ${\widehat{\mathit{U}}}_{k-1}$ arises due to this discrepancy in the vehicle’s center of gravity position and is defined to have an element in the i^th row and j^th column given by [26]

$$\begin{array}{c}\hfill {\widehat{U}}_{k-1}(i,j)=\left[{\left.\sum _{m=1}^3\frac{{\partial }^{2}g\left(i\right)}{\partial r\left(j\right)\partial r\left(m\right)}u\left(m\right)\right|}_{\mathit{r}={\widehat{\mathit{r}}}_{cg,k-1}}\right],\end{array}$$

where

$$\begin{array}{c}\hfill {\mathbf{u}}_{k-1}={\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\Delta {\widehat{\mathbf{\varphi }}}_k.\end{array}$$

It should be noted that $r\left(j\right)$ and $r\left(i\right)$ denote the i^th and j^th elements of the ${\mathit{r}}_{cg,k-1}$ vector, while $g\left(i\right)$ similarly denotes the i^th component of $g_{k-1}$ and $u\left(m\right)$ denotes the m^th component of ${\mathbf{u}}_{k-1}$. Finally, notice that the frame designations have been dropped from the transformation matrix—it is assumed that the transformation matrix $\mathit{T}$ represents the transformation from the inertial frame to the case frame.

3.7. Covariance Propagation

Let the navigation filter state vector ${\widehat{\mathit{x}}}_k$ be given by the concatenation of the estimated position, velocity, and attitude states of the vehicle, augmented by the estimated inertial sensor parameters, or

$$\begin{array}{c}\hfill {\widehat{\mathit{x}}}_k=\left[\begin{array}{c}{\widehat{\mathit{r}}}_{c,k}^i\\ {\widehat{v}}_{c,k}^i\\ {\widehat{\overline{\mathit{q}}}}_{i,k}^c\\ {\widehat{\mathit{p}}}_{g,k}\\ {\widehat{\mathit{p}}}_{a,k}\end{array}\right],{\widehat{\mathit{p}}}_{g,k}=\left[\begin{array}{c}{\widehat{\mathit{b}}}_{g,k}\\ {\widehat{\mathit{s}}}_{g,k}\\ {\widehat{\mathit{m}}}_{g,k}\\ {\widehat{\mathit{n}}}_{g,k}\end{array}\right],\mathrm{and}{\widehat{\mathit{p}}}_{a,k}=\left[\begin{array}{c}{\widehat{\mathit{b}}}_{a,k}\\ {\widehat{\mathit{s}}}_{a,k}\\ {\widehat{\mathit{m}}}_{a,k}\\ {\widehat{\mathit{n}}}_{a,k}\end{array}\right],\end{array}$$

where ${\widehat{\mathit{p}}}_{g,k}$ and ${\widehat{\mathit{p}}}_{a,k}$ are the concatenated sensor error parameters for the gyroscope and accelerometers, respectively. Notice that the state estimation error is defined to be

$$\begin{array}{c}\hfill {\mathit{e}}_k={\mathit{x}}_k-{\widehat{\mathit{x}}}_k\end{array}$$

for all states except for the attitude, which requires the multiplicative error definition for the attitude quaternion in Equation (63). Thus, the dynamics governing the estimation error can, as given in Equation (64), can be expressed as

$$\begin{array}{c}\hfill {\mathit{e}}_k={\mathit{F}}_{k-1}{\mathit{e}}_{k-1}+{\mathit{M}}_{k-1}{\mathit{w}}_{k-1},\end{array}$$

where ${\mathit{F}}_{k-1}$ is the discrete-time dynamics Jacobian that describes the mapping of errors from one time-step to the next, while ${\mathit{M}}_{k-1}$ maps dynamic process noise into the state estimation error. It is worth pointing out that, for systems aided by strapdown inertial sensors, the dynamic process noise described by ${\mathit{w}}_{k-1}$ is taken to be the noise of the inertial sensors; i.e,

$$\begin{array}{c}\hfill {\mathit{w}}_{k-1}=\left[\begin{array}{c}{\mathit{w}}_{a,k-1}\\ {\mathit{w}}_{g,k-1}\end{array}\right].\end{array}$$

After combining Equation (29) with Equation (64), the elements of ${\mathit{F}}_{k-1}$ and ${\mathit{M}}_{k-1}$ are given by inspection. For brevity, this combination is done in Appendix A. From this definition of the state estimation error dynamics, the linearized covariance propagation can be clearly stated such that

$$\begin{array}{c}\hfill {\mathit{P}}_k={\mathit{F}}_{k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{k-1}^T+{\mathit{M}}_{k-1}{\mathit{q}}_{k-1}{\mathit{M}}_{k-1}^T,\end{array}$$

where ${\mathit{Q}}_{k-1}$ is the process noise covariance. Note that, for the typical INS, the uncertainty introduced by noise in strapdown sensor measurements is commonly incorporated as process noise.

4. Simulation

Two separate architectures are examined for a lunar descent-to-landing simulation. The first architecture utilizes a traditional dead-reckoning architecture—i.e., one propagating after receiving a single incremental angle and velocity [22,26]—while the other employs the CSS algorithms and utilizes the rigorous treatment of the estimation error propagation as developed within Section 3. The same trajectory is examined in [22,37], where the effects of external measurements on the navigation performance are considered. In contrast, the analysis here focuses on the situation where the vehicle is only navigating through inertial navigation techniques and thus has no external measurements to aid in navigation. Understanding how the estimation error propagates through the navigation system in a high-stakes scenario, such as a lunar landing, is crucial for informing future systems’ development. Comparing the two different propagation techniques with this scenario provides a quantifiable differentiation between navigation systems equipped with the CSS algorithms’ error propagation and those without. It should be noted that the selected trajectory is chosen not to maximize the effects or usefulness of CSS algorithms but to represent a scenario in which these algorithms may be used on a realistic trajectory in which increased accuracy and precision is desired. This comparison highlights the expected difference in navigation system performance when employing the error propagation alongside the often-implemented CSS algorithms.

To assess performance and compare configurations, Monte Carlo analysis is used. However, this method of analysis typically samples the true state from some distribution about the mean. Breaking convention, the true position, velocity, attitude, acceleration, and angular velocity are fixed for this trajectory, as is the case in [37], requiring that the initial estimates be sampled from a distribution about the true states. The initial states are assumed to be initially uncorrelated and sampled from Gaussian distributions, with $1\sigma$ uncertainties in each channel of position, velocity, and attitude shown in Table 1.

Table 1. Initial uncertainty for each state (per axis, 1$\mathbf{\sigma }$).

	Uncertainty (1$\mathit{\sigma }$)
Position	1000 m
Velocity	0.1 $\mathrm{m}/\mathrm{s}$
Attitude	100 arcsec

Aspects of the true vehicle trajectory are illustrated in Figure 2, Figure 3 and Figure 4. The altitude profile of the vehicle across the mission is shown in Figure 2, where the trajectory is initialized 50 km above the lunar surface. The vehicle slowly descends over the first 24 min to an altitude of 16.5 km and enters a powered descent phase after 25.5 min of mission elapsed time (MET). During powered descent, the vehicle rapidly descends to the surface in just under 7 min. Figure 3 shows the vehicle attitude, expressed as Euler angles; the attitude profile is only provided for a portion of the mission to show the changes experienced throughout the powered descent phase of the simulation. A large attitude maneuver occurs at approximately 24 min MET, as can be seen in Figure 3 and Figure 4, while a smaller attitude maneuver occurs at roughly 25.5 min MET, coinciding with the start of the powered descent phase. During the powered descent phase, the vehicle performs a translational maneuver to decelerate and land the vehicle, as seen in Figure 5. As the simulation ends and the vehicle approaches its landing location, several small attitude correction maneuvers are also performed.

Figure 2. Vehicle altitude during terminal descent.

Figure 3. Vehicle attitude (Euler angles) during terminal descent.

Figure 4. Vehicle angular velocity magnitude during terminal descent.

Figure 5. Non-gravitational acceleration magnitude during terminal descent.

The IMU gyroscope and accelerometer measurements utilize the model given in Section 2.5 and have sensor error statistics consistent with the Northrop Grumman LN-200S [38], as provided in Table 2. Measurement error sources of white noise, bias, scale factor, misalignment, and nonorthogonality are included for both the gyro and accelerometer measurements. Each error is sampled from a zero-mean Gaussian distribution defined by the statistics in Table 2; the bias, scale factor, misalignment, and nonorthogonality sources are modeled to be constant throughout a given trial. Because the primary goal of this work is to compare the propagation architectures, no external measurements are modeled, and only the internal measurements provided by the IMU are simulated. As such, only the prediction stage of the multiplicative extended Kalman filter is employed.

Table 2. Error specifications for the LN-200S IMU (per axis, 1$\mathbf{\sigma }$).

	Gyroscope	Accelerometer
Frequency	400 Hz	400 Hz
Noise	0.07 ${}^{\circ }/\sqrt{\mathrm{hr}}$	35 $\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{hr}}$
Bias	1 ${}^{\circ }/\mathrm{hr}$	300 $\mathsf{\mu }\mathrm{g}$
Scale factor	300 ppm	100 ppm
Misalignment	0.1 mrad	0.1 mrad
Nonorthogonality	0.1 mrad	0.1 mrad

4.1. Nominal Simulation

The nominal state propagation considers the case in which the state is propagated at 400 Hz—the frequency at which inertial measurements are sampled. Processing a single measurement is a generally desirable approach for the navigation system, as the algorithmic mechanics tend to be simpler. However, it is almost always desirable to process higher-rate IMU data so that underlying vibration in the vehicle motion can be detected. Unfortunately, the computational resources necessary to process inertial measurements at the rate at which modern inertial sensors are capable of producing them are significant, even when considering state-of-the-art computing systems. Considering this case for the nominal simulation gives a baseline performance for the navigation system’s state and covariance propagation, as it exemplifies the most common method of inertial navigation.

4.2. Coning, Sculling, and Scrolling Simulation

The proposed architecture, which employs the CSS algorithms as presented in Section 2 and the error propagation derived within Section 3, can also be used to propagate the mean and covariance of the vehicle state. For simulation, these algorithms operate at a frequency of 10 Hz, while the measurements are simulated at 400 Hz. Therefore, a batch of 40 measurements obtained between $t_{k-1}$ and $t_k$ is used to propagate the state and uncertainty of the vehicle through the utilization of second-order CSS correction algorithms. The dynamics outlined in Equation (21) describe the vehicle state evolution.

5. Results and Discussion

For each simulation, the Monte Carlo sample covariance and averaged filter covariance are examined and compared against one another to determine the statistical consistency of the chosen method; i.e., either CSS or the traditional single-measurement dead-reckoning. To better determine how the developed CSS error propagation affects the uncertainty propagation, the same statistics are then compared directly against one another. In Figure 6 and Figure 7, results for the position, velocity, and attitude estimation errors from the nominal simulation are shown. Figure 6 shows the mean estimation error, alongside the averaged filter covariance and Monte Carlo sample covariance 3$\sigma$ intervals for each component of the position, velocity, and attitude, whereas Figure 7 shows the RSS value for each. Figure 8 and Figure 9 show similar results for the application of the CSS algorithms. From these figures, it is clear that both configurations are consistent with the Monte Carlo statistics, despite the presence of a bias in the mean error. However, by comparing Figure 7 and Figure 9, it is observed that both methods produce similar mean estimation errors with no noticeable difference in the predicted or observed uncertainty intervals. It is worth noting that the mean error not being zero is likely due to under-sampling and continues to reduce with an increasing number of Monte Carlo trials.

Figure 6. Monte Carlo simulation results from 500 trials using traditional methods of inertial navigation: mean error, averaged filter covariance ($3\sigma$), and Monte Carlo sample covariance ($3\sigma$) for position (top), velocity (middle), and attitude (bottom).

Figure 7. Monte Carlo simulation results from 500 trials using traditional methods of inertial navigation: mean error (RSS), averaged filter covariance ($1\sigma$, RSS), and Monte Carlo sample covariance ($1\sigma$, RSS) for position (top), velocity (middle), and attitude (bottom).

Figure 8. Monte Carlo simulation results from 500 trials using CSS corrections for inertial navigation: mean error, averaged filter covariance ($3\sigma$), and Monte Carlo sample covariance ($3\sigma$) for position (top), velocity (middle), and attitude (bottom).

Figure 9. Monte Carlo simulation results from 500 trials using CSS corrections for inertial navigation: mean error (RSS), averaged filter covariance ($1\sigma$, RSS), and Monte Carlo sample covariance ($1\sigma$, RSS) for position (top), velocity (middle), and attitude (bottom).

Within Figure 10 and Figure 11, a direct comparison of each method is made by examining the averaged normalized estimation error squared (ANEES) and a normalized error between the RSS standard deviations. The ANEES, $\overline{ϵ}$, is calculated by [39,40]

$$\begin{array}{c}\hfill \overline{ϵ}=\frac{1}{nM}\sum _{i=1}^M{ϵ}_i=\frac{1}{nM}\sum _{i=1}^M{({\mathit{x}}_i-{\widehat{\mathit{x}}}_i)}^T{\mathit{P}}_i^{-1}({\mathit{x}}_i-{\widehat{\mathit{x}}}_i)=\frac{1}{nM}\sum _{i=1}^M{\mathit{e}}_i^T{\mathit{P}}_i^{-1}{\mathit{e}}_i,\end{array}$$

which is the squared Mahalanobis distance for trial i of the estimation error, ${\mathit{e}}_i$, with respect to the filter covariance, ${\mathit{P}}_i$, averaged over the number of states, n, and the number of Monte Carlo trials, M. The ANEES measure is ${\chi }^2$-distributed if the estimation errors are Gaussian-distributed and allows the filter to be rejected as credible at a particular level, $\alpha$, should a credibility interval be breached for a significant amount of time. The credibility interval is constructed such that $Pr\left(\overline{ϵ}\in [a,b]|nM\overline{ϵ}\sim {\chi }_{nM}^2\right)=1-\alpha$ for $a<1<b$ and $0<\alpha \ll 1$, where ${\chi }_{nM}^2$ is a ${\chi }^2$-distribution of $nM$ degrees of freedom [40]. The interval $[a,b]$ contains 95% of the probability mass for the ${\chi }^2$ distribution having a mean of one and $nM$ degrees of freedom when $\alpha =0.05$. The lower bound a separates the lower $\alpha /2$ of the probability mass, while the upper bound eliminates the upper $\alpha /2$. If $\overline{ϵ}=1$, the ANEES is perfectly consistent with the error distribution. It is necessary to note that ANEES is not a credibility measure but is useful in recognizing if the filter’s approximation of the uncertainty is representative of the errors or that the filter is consistent [41]. Finally, this measure allows the recognition of estimation performance; the estimator overestimates the estimation error when the ANEES is less than one and underestimates the error when the ANEES is greater than one. While examining the consistency of the diagonal elements of the filter and Monte Carlo sample covariance matrices is useful, ANEES allows the direct comparison of cross-correlation terms in the covariance structure to the estimation errors. Figure 10 shows the ANEES for position, velocity, and attitude for both configurations and allows the assessment that each estimator has approximately the same level of credibility for the estimation of those states. While no significant difference manifests, notice that a slight deviation occurs in the position ANEES around 28 min MET, though no significant difference is noticed in the consistency of the estimators.

Figure 10. ANEES comparison for Monte Carlo estimation errors generated by CSS and traditional inertial navigation strategies for position (top), velocity (middle), and attitude (bottom).

Figure 11. Normalized RSS standard deviation error between CSS and high-frequency traditional dead-reckoning averaged filter and Monte Carlo sample covariances for position (top), velocity (middle), and attitude (bottom).

Figure 11 compares the RSS standard deviations of the Monte Carlo sample covariance and averaged filter covariance, ${\sigma }_{x,c}^{rss}$ and ${\sigma }_{x,t}^{rss}$ respectively, by computing a normalized error, here defined as

$$\begin{array}{c}\hfill e_{\sigma ,x}=\frac{{\sigma }_{x,c}^{rss}-{\sigma }_{x,t}^{rss}}{{\sigma }_{x,t}^{rss}},\end{array}$$

between the CSS and traditional configurations for a state x, such as position, velocity, or attitude. By this measure, if the value is less than zero, the CSS methods predict or observe a smaller spread in the distribution than the traditional methods and a larger spread if it is greater than zero. Even though the differences are small, several comments can be made when comparing system performance. By recognizing that the ratios of Monte Carlo sample standard deviations are less than zero for the velocity and position distributions and that the mean error is consistent between the two configurations, as recognized by comparing Figure 7 and Figure 9, it is concluded that the application of the CSS algorithms successfully reduces the error present in the system. The same trend is followed by the averaged filter ratio until approximately 24 min MET, when the attitude maneuver occurs. After the attitude maneuver, the vehicle begins the powered-descent phase, and a continued divergence between the ratios is observed. It is therefore clear that the introduction of these algorithms allows for a slightly more conservative representation of the uncertainty during maneuvers, while simultaneously producing a smaller spread in the estimation errors. A caveat to the analysis is that the sample size of 500 Monte Carlo trials is likely somewhat under-sampled, and the significance of the findings made by examining the ratios is difficult to justify as the differences are relatively small.

Comparing the run-time for 500 Monte Carlo trials, a mean run-time of 15.5 min is found for the traditional method and a time of 2.94 min is found for the CSS configuration. The required run-time to process the same measurements and provide the state and covariance at a down-sampled frequency yielded an approximately 81% reduction in computational complexity and scaled as the frequency of covariance propagation is further reduced. Unfortunately, the time savings do not seem to be as large as those shown in [34]. This may be a result of the relatively low number of measurements processed by the CSS algorithms between state predictions; if higher-frequency measurements are available, the reduction is likely to be more significant. Finally, it should be noted that an attempt at streamlining the implementation may also yield an additional run-time reduction.

6. Conclusions

Advancements in precision navigation systems are needed to enable the increasingly complex and demanding tasks required for future autonomous vehicles. One method to consider for the improvement of current navigation system capabilities—for the estimation of position, velocity, attitude, and other vehicle-relevant parameters—is the incorporation of previously unmodeled effects, such as the contribution of coning, sculling, and scrolling (CSS) corrections on vehicle position, velocity, and attitude uncertainty within the popular strapdown inertial navigation system (SINS). A method for developing each algorithm’s error propagation has been presented alongside the resulting expressions for a set of CSS algorithms. It is worth noting that the developed error propagation relies upon a linearized error propagation, developed by truncating higher-order terms and assuming that the non-gravitational acceleration and angular velocity vary linearly with time. Thus, this approach is a first-order approximation of the true CSS error propagation—future developments that relax the associated simplifications by incorporating the higher-order effects and allowing more optimized approaches, such as those discussed in [15,25], are of interest for future work.

It is found through Monte Carlo simulation and analysis that the inclusion of an error propagation for the CSS algorithms results in a more conservative filter representation of the predicted position, velocity, and attitude uncertainties following a large attitude maneuver. Additionally, the averaged estimation errors seem consistent with existing methods for the simulated scenario, where both the CSS and traditional dead-reckoning approaches exhibit the same level of statistical consistency. It is also shown that reduced computationally complexity is established by the CSS system when compared to traditional methods of discrete dead-reckoning, giving an 81% reduction in average run-time. If the generation of the error mappings was handled by a navigation pre-processor or within the IMU software, a further reduction of the computational complexity would be seen. Finally, because the developed models for uncertainty propagation allow uncertainty to be handled in a more mathematically consistent manner when using CSS corrections, while simultaneously producing a computationally efficient and statistically conservative navigation filter, it can be concluded that the developed models could be beneficial to future navigation systems.

Appendix A.1. Attitude

Considering the gyro measurements to be corrupted by bias, scale factor, misalignment, nonorthogonality, and noise error sources, the propagation of these errors into the attitude estimate is given by the combination of Equations (29a) and (64c). Assuming that the parametric errors for the gyroscope have constant dynamics, i.e., that

$$\begin{array}{c}\hfill {\mathit{b}}_{g,k}={\mathit{b}}_{g,k-1},{\mathit{s}}_{g,k}={\mathit{s}}_{g,k-1},{\mathit{m}}_{g,k}={\mathit{m}}_{g,k-1},\mathrm{and}{\mathit{n}}_{g,k}={\mathit{n}}_{g,k-1},\end{array}$$

the component of Equation (64c) containing the measurement errors is given by

$$\begin{array}{cc}\hfill \sum _{i=1}^{\ell }\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}\right){\mathit{e}}_{\Delta \theta ,i}=& -{\mathit{L}}_{s_g}{\mathit{e}}_{s_g,k}+{\mathit{L}}_{m_g}{\mathit{e}}_{m_g,k}-{\mathit{L}}_{n_g}{\mathit{e}}_{n_g,k}-{\mathit{L}}_{b_g}{\mathit{e}}_{b_g,k}-{\mathit{w}}_{a,g},\hfill \end{array}$$

(A1)

where the components

$$\begin{array}{cc}\hfill {\mathit{L}}_{b_g}& =\ell {\mathit{I}}_{3\times 3}+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}\hfill \end{array}$$

(A2a)

$$\begin{array}{cc}\hfill {\mathit{L}}_{s_g}& =[{\mathbf{\theta }}_{m,\ell }\setminus ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta {\mathbf{\theta }}_{m,i}\setminus ]\hfill \end{array}$$

(A2b)

$$\begin{array}{cc}\hfill {\mathit{L}}_{m_g}& =[{\mathbf{\theta }}_{m,\ell }\times ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta {\mathbf{\theta }}_{m,i}\times ]\hfill \end{array}$$

(A2c)

$$\begin{array}{cc}\hfill {\mathit{L}}_{n_g}& =[{\mathbf{\theta }}_{m,\ell }\ast ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta {\mathbf{\theta }}_{m,i}\ast ]\hfill \end{array}$$

(A2d)

$$\begin{array}{cc}\hfill {\mathit{w}}_{a,g}& =\sum _{i=1}^{\ell }\left({\mathit{I}}_{3\times 3}+{\mathsf{\Xi }}_{con,i}\right){\mathit{w}}_{g,i}.\hfill \end{array}$$

(A2e)

are defined to simplify the notation and isolate the propagation of the gyro bias, scale factor, misalignment, nonorthogonality, and noise into the attitude estimate, respectively. Note that ${\mathbf{\theta }}_{m,\ell }$ is simply the sum of the measurements; i.e., ${\mathbf{\theta }}_{m,\ell }={\sum }_{i=1}^{\ell }\Delta {\mathbf{\theta }}_{m,i}$. The attitude error, including the contribution from each of the gyro error sources, is thus

$$\begin{array}{cc}\hfill {\mathit{e}}_{a,k}& =\mathit{T}(\Delta {\widehat{\mathbf{\varphi }}}_k){\mathit{e}}_{a,k-1}-{\mathit{L}}_{s_g}{\mathit{e}}_{s_g,k}+{\mathit{L}}_{m_g}{\mathit{e}}_{m_g,k}-{\mathit{L}}_{n_g}{\mathit{e}}_{n_g,k}-{\mathit{L}}_{b_g}{\mathit{e}}_{b_g,k}-{\mathit{w}}_{a,g}.\hfill \end{array}$$

(A3)

It is important to note that the noise is not assumed to be constant over the coning interval and requires special attention for implementation.

Appendix A.2. Velocity

Given that both the gyros and accelerometers can be corrupted by bias, scale factor, misalignment, nonorthogonality, and noise error sources, the error in the velocity estimate will also be dependent upon the sensor error parameters. Combining Equation (29b) with Equation (64b), the component mapping errors in the accelerometer measurements to the velocity becomes

$$\begin{array}{cc}\hfill \sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T\left({\mathbf{N}}_{\theta }+{\mathsf{\Xi }}_{con,i}\right){\mathit{e}}_{\Delta {\mathit{V}}_i,k}=& -{\mathit{V}}_{s_a}{\mathit{e}}_{s_a,k}+{\mathit{V}}_{m_a}{\mathit{e}}_{m_a,k}-{\mathit{V}}_{n_a}{\mathit{e}}_{n_a,k}-{\mathit{V}}_{b_a}{\mathit{e}}_{b_a,k}-{\mathit{w}}_{v,a},\hfill \end{array}$$

(A4)

where

$$\begin{array}{cc}\hfill {\mathit{V}}_{s_a}& ={\widehat{\mathit{T}}}_{k-1}^T\left\{{\mathbf{N}}_{\theta }[v_{m,\ell }\setminus ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta v_{m,i}\setminus ]\right\}\hfill \end{array}$$

(A5a)

$$\begin{array}{cc}\hfill {\mathit{V}}_{m_a}& ={\widehat{\mathit{T}}}_{k-1}^T\left\{{\mathbf{N}}_{\theta }[v_{m,\ell }\times ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta v_{m,i}\times ]\right\}\hfill \end{array}$$

(A5b)

$$\begin{array}{cc}\hfill {\mathit{V}}_{n_a}& ={\widehat{\mathit{T}}}_{k-1}^T\left\{{\mathbf{N}}_{\theta }[v_{m,\ell }\ast ]+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}[\Delta v_{m,i}\ast ]\right\}\hfill \end{array}$$

(A5c)

$$\begin{array}{cc}\hfill {\mathit{V}}_{b_a}& ={\widehat{\mathit{T}}}_{k-1}^T\left(\ell {\mathbf{N}}_{\theta }+\sum _{i=1}^{\ell }{\mathsf{\Xi }}_{con,i}\right)\hfill \end{array}$$

(A5d)

$$\begin{array}{cc}\hfill {\mathit{w}}_{v,a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T\left({\mathbf{N}}_{\theta }+{\mathsf{\Xi }}_{con,i}\right){\mathit{w}}_{a,i},\hfill \end{array}$$

(A5e)

and

$$\begin{array}{c}\hfill {\mathbf{N}}_{\theta }={\mathit{I}}_{3\times 3}+\frac{1}{2}\left[{\widehat{\mathbf{\theta }}}_{\ell }\times \right]\end{array}$$

(A6)

are defined to simplify notation. Note that this results from assuming that the bias, scale factor, misalignment, and nonorthogonality errors are constant over the major interval, though this assumption can be easily relaxed.

Similarly, by combining Equation (29a) with Equation (64b), defining the mapping of each error in the gyro measurement into the velocity error as

$$\begin{array}{cc}\hfill v_{b_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathsf{\Xi }}_{scul,i}-\frac{1}{2}\left({\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{v}}_{\ell }\times \right]+\Delta t_k{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{s_b}\right)\hfill \end{array}$$

(A7a)

$$\begin{array}{cc}\hfill v_{s_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathsf{\Xi }}_{scul,i}[\Delta {\mathbf{\theta }}_{m,i}\setminus ]\hfill \\ \hfill & -\frac{1}{2}\left({\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{v}}_{\ell }\times \right][{\mathbf{\theta }}_{m,\ell }\setminus ]+\Delta t_k{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{s_g}\right)\hfill \end{array}$$

(A7b)

$$\begin{array}{cc}\hfill v_{m_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathsf{\Xi }}_{scul,i}[\Delta {\mathbf{\theta }}_{m,i}\times ]\hfill \\ \hfill & -\frac{1}{2}\left({\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{v}}_{\ell }\times \right][{\mathbf{\theta }}_{m,\ell }\times ]+\Delta t_k{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{m_g}\right)\hfill \end{array}$$

(A7c)

$$\begin{array}{cc}\hfill v_{n_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathsf{\Xi }}_{scul,i}[\Delta {\mathbf{\theta }}_{m,i}\ast ]\hfill \\ \hfill & -\frac{1}{2}\left({\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{v}}_{\ell }\times \right][{\mathbf{\theta }}_{m,\ell }\ast ]+\Delta t_k{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{n_g}\right)\hfill \end{array}$$

(A7d)

$$\begin{array}{cc}\hfill {\mathit{w}}_{v,g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T\left({\mathsf{\Xi }}_{scul,i}-\frac{1}{2}\left[{\widehat{v}}_{\ell }\times \right]\right){\mathit{w}}_{g,i}-\frac{\Delta t_k}{2}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{w}}_{a,g},\hfill \end{array}$$

(A7e)

and incorporating the definitions from Equation (A5), it can be shown that the velocity error propagation is

$$\begin{array}{cc}\hfill {\mathit{e}}_{v,k}=& \Delta t_k\left({\widehat{G}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\mathit{e}}_{r,k-1}+{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{v}}_{ng,k}\times \right]\hfill \\ \hfill & -\Delta t_k\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{\mathit{r}}}_{cg/c,k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\Delta t_k\left\{\left({\widehat{g}}_{k-1}-\frac{1}{2}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{2}{\widehat{g}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{d,k-1}\hfill \\ \hfill & -v_{s_a}{\mathit{e}}_{s_a,k}+v_{m_a}{\mathit{e}}_{m_a,k}-v_{n_a}{\mathit{e}}_{n_a,k}-v_{b_a}{\mathit{e}}_{b_a,k}-{\mathit{w}}_{v,a}\hfill \\ \hfill & -v_{s_g}{\mathit{e}}_{s_g,k}+v_{m_g}{\mathit{e}}_{m_g,k}-v_{n_g}{\mathit{e}}_{n_g,k}-v_{b_g}{\mathit{e}}_{b_g,k}-{\mathit{w}}_{v,g}.\hfill \end{array}$$

(A8)

Therefore, the error propagation in Equation (A8) describes the error propagation for the velocity estimate given that the navigation system is dependent upon strapdown inertial sensors and uses a coning and sculling algorithm, where the sensor errors are assumed constant over the timestep.

Appendix A.3. Position

With the definition of the velocity error dynamics written in terms of the strapdown sensor errors in Equation (A8), their propagation into the position error follows similarly. The mappings defining the propagation of gyro measurement errors into the position estimate are thus defined as

$$\begin{array}{cc}\hfill {\mathit{r}}_{b_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}-\frac{\Delta t_k^2}{6}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{b_g}\hfill \end{array}$$

(A9a)

$$\begin{array}{cc}\hfill {\mathit{r}}_{s_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}\left[\Delta v_{m,i}\setminus \right]-\frac{\Delta t_k^2}{6}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{s_g}\hfill \end{array}$$

(A9b)

$$\begin{array}{cc}\hfill {\mathit{r}}_{m_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}\left[\Delta v_{m,i}\times \right]-\frac{\Delta t_k^2}{6}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{m_g}\hfill \end{array}$$

(A9c)

$$\begin{array}{cc}\hfill {\mathit{r}}_{n_g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}[\Delta v_{m,i}\ast ]-\frac{\Delta t_k^2}{6}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{L}}_{n_g}\hfill \end{array}$$

(A9d)

$$\begin{array}{cc}\hfill {\mathit{w}}_{r,g}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta \theta ,i}{\mathit{w}}_{a,i}-\frac{\Delta t_k^2}{6}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]{\mathit{w}}_{a,g}.\hfill \end{array}$$

(A9e)

while the mapping for the errors in the accelerometer measurements are defined as

$$\begin{array}{cc}\hfill {\mathit{r}}_{b_a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}\hfill \end{array}$$

(A10a)

$$\begin{array}{cc}\hfill {\mathit{r}}_{s_a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}\left[\Delta {\mathbf{\theta }}_{m,i}\setminus \right]\hfill \end{array}$$

(A10b)

$$\begin{array}{cc}\hfill {\mathit{r}}_{m_a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}\left[\Delta {\mathbf{\theta }}_{m,i}\times \right]\hfill \end{array}$$

(A10c)

$$\begin{array}{cc}\hfill {\mathit{r}}_{n_a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}[\Delta {\mathbf{\theta }}_{m,i}\ast ]\hfill \end{array}$$

(A10d)

$$\begin{array}{cc}\hfill {\mathit{w}}_{r,a}& =\sum _{i=1}^{\ell }{\widehat{\mathit{T}}}_{k-1}^T{\mathit{X}}_{\Delta v,i}{\mathit{w}}_{a,i}.\hfill \end{array}$$

(A10e)

With the mappings defined in Equations (A9) and (A10), the error propagation for the position becomes

$$\begin{array}{cc}\hfill {\mathit{e}}_{r,k}=& \left\{{\mathit{I}}_{3\times 3}+\frac{\Delta t_k^2}{2}\left({\widehat{G}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right)\right\}{\mathit{e}}_{r,k-1}+\Delta t_k{\mathit{e}}_{v,k-1}\hfill \\ \hfill & +\{\frac{\Delta t_k^2}{6}{\widehat{\mathit{T}}}_{k-1}^T\left[\left({\widehat{d}}_{k-1}\times \Delta {\widehat{\mathbf{\varphi }}}_k\right)\times \right]-{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathit{r}}}_{ng,k}\times \right]\hfill \\ \hfill & -\frac{\Delta t_k^2}{2}\left({\widehat{G}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T\left[{\widehat{d}}_{k-1}\times \right]\}{\mathit{e}}_{a,k-1}\hfill \\ \hfill & +\frac{\Delta t_k^2}{2}\left\{\left({\widehat{G}}_{k-1}-\frac{1}{3}{\widehat{\mathit{U}}}_{k-1}\right){\widehat{\mathit{T}}}_{k-1}^T+\frac{1}{3}{\widehat{G}}_{k-1}{\widehat{\mathit{T}}}_{k-1}^T\left[\Delta {\widehat{\mathbf{\varphi }}}_k\times \right]\right\}{\mathit{e}}_{d,k-1}\hfill \\ \hfill & -{\mathit{r}}_{s_a}{\mathit{e}}_{s_a,k}+{\mathit{r}}_{m_a}{\mathit{e}}_{m_a,k}-{\mathit{r}}_{n_a}{\mathit{e}}_{n_a,k}-{\mathit{r}}_{b_a}{\mathit{e}}_{b_a,k}-{\mathit{w}}_{r,a}\hfill \\ \hfill & -{\mathit{r}}_{s_g}{\mathit{e}}_{s_g,k}+{\mathit{r}}_{m_g}{\mathit{e}}_{m_g,k}-{\mathit{r}}_{n_g}{\mathit{e}}_{n_g,k}-{\mathit{r}}_{b_g}{\mathit{e}}_{b_g,k}-{\mathit{w}}_{r,g}.\hfill \end{array}$$

(A11)

Equation (A11) the error dynamics for the position estimate, influenced by accelerometer and gyro bias, scale factor, misalignment, nonorthogonality, and white noise—the bias, scale factor, misalignment, and nonorthogonality sensor error are assumed to be constant from $t_{k-1}$ to $t_k$. The noise, however, is not assumed constant over the interval and thus provides a separate contribution from each measurement, requiring the definition of ${\mathit{w}}_{r,g}$ and ${\mathit{w}}_{r,a}$.