GPS-denied navigation for small unmanned aircraft systems (UASs) is an active and rich field of research with significant practical applications such as infrastructure inspection and security. Most UASs fuse GPS with accelerometer and rate-gyro data to provide accurate global state estimates suitable for feedback control. When GPS is not available, however, additional sensors such as cameras or lidar are required. Owing to the size, weight, and power constraints and fast vehicle dynamics associated with small UASs, many such systems incorporate these additional sensors using filter-based estimation techniques rather than traditional full simultaneous localization and mapping (SLAM) algorithms. Filter-based approaches are computationally efficient and ensure smooth, timely state estimates for control.
In the absence of GPS updates, many filtering methods utilize incremental odometry measurements from either visual odometry or laser scan matching. These odometry measurements can be computed frame-to-frame, or several measurements can be computed with respect to the same keyframe image or scan. The keyframe image or scan is updated when there is insufficient overlap with current images or scans to compute reliable odometry measurements. Keyframe-based approaches have the advantage of reducing temporal drift in the odometry measurements (Leutenegger et al., 2015).
Despite having only incremental measurements available, the majority of GPS-denied navigation approaches directly estimate the vehicle's global pose with respect to Fig. 1. Block diagram of the general relative navigation system architecture. Flight critical estimation and control is performed with respect to a local frame. The framework is described in more detail by Wheeler et al. (2020). some fixed origin. Without global position measurements, however, the vehicle's global pose and heading are unobservable (Martinelli, 2012;Weiss et al., 2012;Jones et al., 2007). As a result, global filters can suffer from inconsistent and unbounded state uncertainties, erratic state jumps when applying relative measurements, and the inability to directly apply intermittent global information without causing large state jumps (Julier and Uhlmann, 2001;Bailey and Durrant-Whyte, 2006;Kottas et al., 2013;Wheeler et al., 2018). In contrast, the relative navigation approach estimates only the relative state of the vehicle with respect to the location of the most recent odometry keyframe (Leishman et al., 2014b). As a result, the odometry provides direct measurements of the position and heading states, making them observable by construction and thereby ensure consistent and bounded state estimates. Each time a new keyframe is declared, the current state and covariance estimates are passed to a back-end map that concatenates them as edges in a pose graph to reconstruct the global path of the vehicle. The position and heading states in the filter are then reset to zero and estimation continues. The relative navigation architecture is illustrated graphically in Figure 1, and parallels ideas found in the SLAM literature (Chong and Kleeman, 1999;Kim et al., 2010).
Recent simulation results have shown that using the relative navigation framework to ensure observability provides significant advantages in terms of consistency of the estimated relative and global states, as well as some improvement in accuracy and system robustness when incorporating intermittent global information (Wheeler et al., 2018). Furthermore, recent multirotor hardware flight test results have demonstrated the effectiveness of relative navigation for prolonged GPS-degraded navigation of small UASs (Wheeler et al., 2020). This article compliments the work in Wheeler et al. (2018) and Wheeler et al. (2020) by providing a complete, self-contained, tutorial derivation of the relative state-estimation filter used in those papers to obtain hardware results. While those papers provide thorough theoretical and practical motivation for the relative navigation approach, the core relative estimator component itself has only been briefly described in the literature to date.
The purpose of this article is to fill that gap. The flight tests presented by Wheeler et al. (2018) and Wheeler et al. (2020) successfully leveraged the relative multiplicative extended Kalman filter (RMEKF) presented in this article. While that work demonstrated the effectiveness of the relative navigation framework and gave an overview of the various components, it did not describe the RMEKF in detail. The RMEKF builds upon the multiplicative extended Kalman filter (MEKF), which uses a quaternion to represent attitude and quaternion multiplication to define attitude error. The RMEKF extends the MEKF by defining the UAS state to be with respect to a local coordinate frame associated with the current keyframe image. To accommodate this relative state, the RMEKF introduces an additional keyframe reset step that is applied each time a new keyframe is declared.
This article contributes to the literature in three ways. First, the article provides a tutorial derivation of the MEKF for UAS state estimation given Hamilton quaternions. Second, the article presents a complete derivation of the RMEKF for multirotor UASs, including several important extensions to the original presentation by Leishman and McLain (2014). Third, the article provides a thorough derivation of the RMEKF for both inertial and body-fixed (robocentric) state representations, highlighting the subtle but important differences that exist between the two methods. The following paragraphs describe these contributions in further detail and relate how they compare to the existing literature.
Significant portions of the article are tutorial in nature, clearly motivating why an indirect or error state formulation is necessary when quaternions are used to represent attitude, and providing complete explanations of each step in the derivation of the filter equations. The MEKF was first introduced by Lefferts et al. (1982), and several in-depth discussions and derivations of MEKF implementations have been published (Markley, 2003;Sola, 2016;Trawny and Roumeliotis, 2005). Some of these valuable publications are of similar scope to the current work, but this article provides several meaningful extensions. First, this article derives an estimator for the full state of a UAS (position, velocity, attitude, accelerometer biases, and gyroscope biases), while most previous MEKF papers of similar scope focus only on the attitude and bias estimation. Sola (2016) did derive a full-state MEKF, but this article provides the derivation for a unique set of propagation and measurement models. Second, this article derives the MEKF using the Hamilton quaternion convention as opposed to the JPL convention used in some other works. While the choice of quaternion convention does not fundamentally change the problem, Hamilton quaternions are commonly used in the robotics literature and subtle but important differences arise. This article provides a contrasting perspective to help deepen understanding of quaternions, and aims to clarify some of the confusion that can arise due to the varying conventions used in different works that are not always defined explicitly. Third, the tutorial nature of this article provides sufficient context for the derivation of several new properties relating to quaternions, their error representations, and their Euler-angle decomposition. These properties play a key role in the derivation of the RMEKF to allow partial attitude updates.
Another purpose of this article is to provide a thorough derivation of the RMEKF estimator used successfully by Wheeler et al. (2020) for prolonged UAS navigation in GPS-degraded environments. The RMEKF presented in this article extends the original RMEKF derivation by Leishman and McLain (2014) in several important ways that have proven necessary for prolonged flight. First, this article presents a new visual odometry measurement model, laser scan-matching measurement model, and keyframe reset operation, that together ensure the state remains observable in GPS-denied environments. Second, several novel properties of error quaternions are derived that enable partial updates to quaternion states and their covariances. Third, new terms are added to the state vector in this article to correctly account for uncertainty in the roll, pitch, and altitude of the vehicle at the time a keyframe is declared. Finally, smaller differences include reversing the direction of the odometry measurement model to avoid unnecessarily coupling heading uncertainty into the update, and estimating the global height of the vehicle above ground rather than treating altitude as a relative state.
Another unique contribution of this article is the derivation of the RMEKF when the state is defined with respect to either an inertial frame or a body-fixed robocentric frame. Vehicle dynamics are traditionally expressed in an inertially fixed, gravity-aligned frame (see Leishman et al. (2014b) and Leishman and McLain (2014)). This makes sense especially when the sensors are inertial sensors such as GPS. However, expressing the dynamics in a robocentric frame is often more natural when using robocentric sensors such as cameras and laser range finders, and can help address some of the inconsistency issues of EKF-SLAM (Bloesch et al., 2015;Castellanos et al., 2007) when only relative states are required. A related contribution of this article is a presentation of the subtle differences that arise between using an inertial and robocentric reference frame. These differences can cause confusion in the literature where a side-by-side comparison does not currently exist. For example, in addition to the change in dynamics, subtle changes appear in the quaternion integration, error state definition, measurement models, and keyframe reset operations. By presenting both formulations side-by-side, these differences are clearly explained.
The final contribution of this article is a complete, selfcontained derivation of the filter and all relevant quaternion properties. The definitions of quaternions and error states used across the current estimation literature differ in subtle ways. When these definitions are not documented thoroughly, it becomes difficult to correctly leverage properties from multiple sources. With its tutorial nature and step-by-step explanations, this article is designed to present a complete, self-contained derivation with respect to a consistent, explicitly stated definition. This allows the reader to understand, implement, and potentially modify the RMEKF for new vehicles or applications. Note that while the keyframe reset step and several measurement models are specific to relative navigation, the propagation equations and general filter structure are equally relevant for other applications, such as GPS/inertial navigation system (INS) navigation.
Section 2 summarizes the notation used throughout the article. Section 3 provides an overview of the quaternion definitions used in the article. Specifically, Section 3.8 derives several relevant, new properties of error quaternions. Section 4 outlines the structure of the MEKF. The keyframe reset step is described in Section 5, and an overview of the complete RMEKF algorithm is given. Section 6 derives the specific filter equations for inertial relative navigation (iRN), and Section 7 derives the equations for body-fixed robocentric relative navigation (bRN). Finally, Sections 8 and 9 respectively present results and conclusions.
The following variables, operators, and notation are defined and motivated throughout the article and are summarized here for convenience. Let B denote the vehicle's body frame and I denote an inertial frame.
Quaternions are a common method for representing attitude owing to their improved computational efficiency and accuracy compared with alternative approaches (Casey et al., 2013). A variety of definitions exist for quaternions and their associated operations, leading to subtle discrepancies and potential confusion. The various approaches, described in more detail by Sola (2016), include left-handed versus right-handed quaternion multiplication, active versus passive representations, local-to-global versus globalto-local attitude direction, and quaternion ordering. This section explicitly establishes the definitions and notation used throughout this article and additionally derives several properties required for the filter's derivation. This section is not intended as a complete introduction to quaternions, but rather as a summary of relevant points.
A quaternion q ∈ H is a hyper-complex number of rank four consisting of a scalar and vector portion as q = q 0 + q x i + q y j + q z k.
We use the Hamilton definition of the quaternion, with
(1)
For notational convenience we define the vector portion of the quaternion as q = [q x q y q z ] T , and write the quaternion as
Quaternion multiplication is denoted with the ⊗ operator, and is carried out according to the rules in (1) and standard algebraic multiplication. Using the notation in (2), quaternion multiplication can be written as a matrix multiplication according to
where the operator • is the skew-symmetric operator
The conjugate of a quaternion q is denoted by q * , and is equal to q but with the elements of the vector portion negated. The inverse of a quaternion is given by
The quaternions used in this article all represent rotations and so are unit quaternions, meaning that their norm is 1. Therefore, for unit quaternions we have
Inverting the product of two quaternions results in the product of the inverse of each quaternion in the opposite order, as
In this article, quaternions are denoted passively, meaning that they represent the rotation necessary to express a vector in a different frame. Let quaternion q b a represent the rotation from frame a to frame b and let a y represent a vector expressed in frame a. As described by Kuipers (1999), a y can be expressed in frame b using the quaternion conjugation operation as
The term a y T 0 T is referred to as the pure quaternion constructed from a y.
It is convenient to define an equivalent rotation matrix R(q) such that
An expression for R(q) can be derived by expanding the right-hand side of (7) according to ( 5), (3a), and (3b) as R(q) y 0 = q 0 I -qq qT q 0 y 0 ⊗ q = q 0 y -q y qT y ⊗ q = q 0 I -q q -qT q 0 q 0 y -q y qT y = q 2 0 I -2q 0 q + q qT + q 2 y 0 ,
which implies that R(q) = q 2 0 I -2q 0 q + q qT + q 2 . It can be shown, however, that
Rotation matrices exhibit the following properties:
The formula for vector rotation in Equation ( 6) can be used to derive the manner in which two rotations are compounded together. If q b a defines the rotation from frame a to frame b, and q c b the rotation from frame b to frame c, then to take a vector expressed in frame a and express it in frame c we have
We therefore conclude that
Comparing ( 10) and ( 11), we see that rotation matrices and quaternions compound in the opposite order:
When the quaternion conjugation operation is applied to a quaternion that is not a pure quaternion, the analysis procedure in Equation ( 8) can be repeated to show that
for the same rotation matrix R(q) defined by ( 9). The result of this operation is that the basis of the vector portion of p is rotated by the rotation defined by q. This property is useful when the quaternion is interpreted according to its axis-angle representation, and it is desirable to express the axis vector in a different coordinate frame. This arises in the derivation of some of the measurement models and Jacobians in this article.
Attitude is represented using quaternions of unit length. Unit quaternions do not form a vector space, but rather form a group on the unit sphere S 3 ⊂ H. The group operator is quaternion multiplication and the group of unit quaternions gives a double cover parameterization of the group of rotations SO( 3). Multiplying a unit quaternion by a non-unit quaternion will cause the product to leave the unit sphere. Normalizing the resulting quaternion according to q ← q q returns the quaternion to the unit sphere, but linearization errors are introduced.
To properly rotate a quaternion along the manifold, it is necessary to represent the rotation in terms of a unit quaternion. A rotation can be represented using the unit quaternion as
Rotating a quaternion q along the unit sphere by the rotation θ is accomplished as q ⊗ θ ∧ . This is analogous to the notation q θ and q ⊗ exp( θ 2 ) found in the estimation literature (Hertzberg, et al., 2013). The ∨ operator is defined as
Extracting the underlying rotation between q a and q b is accomplished as ( q a ⊗ q b -1 ) ∨ . This is analogous to the notation log( q a q b ) found in the estimation literature (Hertzberg, et al., 2013). As inverse mappings, it can be shown that θ = θ ∧ ∨ and q = q ∨ ∧ .
Equation ( 15) is undefined when θ equals zero, and in practice becomes numerically unstable as θ approaches zero. There are a number of common approximations of (15) for a small angle δθ , such as the second-order Gibbs vector parameterization (Markley, 2003)
The first-order approximation of both ( 15) and ( 17) is
which is useful when deriving first-order Jacobians. Equation (16) can similarly be approximated for a small quaternion δq as δq ∨ ≈ 2 sign (δq 0 ) δ q.
(19)
Several methods can be used to numerically integrate a quaternion that represents the attitude of a rigid body. Numerical integration is carried out over a finite time step t, and is governed by the angular velocity of the body with respect to an inertial frame as expressed in the body frame, ω B ω B I . Let q t be an incremental quaternion and ω 0 be the nominal angular velocity. Zero-order quaternion integration assumes that the angular velocity is constant over Fig. 2. Quaternion definitions used in the derivation of the quaternion time derivative and integration. The same q t is used for both iRN and bRN because it corresponds to the body-fixed angular velocity ω measured by the rate gyros.
the timestep, ω 0 = ω t , while first-order integration uses linear interpolation, ω 0 = 1 2 ( ω t + ω t-1 ). From (15), we write q t as
The value of the quaternion q t+ t at time t + t can be expressed as the combination of the quaternion q t at time t and the incremental quaternion q t . The manner in which these quaternions are combined depends on whether the attitude quaternion represents the attitude of the body with respect to an inertial frame (iRN) or the attitude of an inertial frame with respect to the body (bRN). As illustrated in Figure 2, the attitude at time t + t for these cases is iRN: q t+ t = q t ⊗ q t , bRN:
where the order of compounding follows Equation (11). Substituting (20) into (21) gives iRN:
bRN:
Integrating according to (22) maintains unit norm, allowing the attitude to propagate on the unit sphere S 3 ⊂ H. In practice, however, this definition becomes numerically unstable as ω 0 approaches zero. As described by Trawny and Roumeliotis (2005), applying L'Hospital's rule to (22a) for iRN shows that lim
Comparing ( 24) to (3b) shows that (23) can be written as
For bRN, a similar analysis can be applied to (22b) to show
Note that for bRN both the order of the quaternion multiplication and the sign of ω 0 have been reversed. In summary, the attitude quaternion is integrated according to (22) when ω 0 is sufficiently large to avoid numerical issues. When ω 0 is small, the integration is approximated as iRN:
bRN:
Integrating according to (25) causes the quaternion to depart from the unit sphere S 3 ; when this method is used a normalization step therefore follows.
The attitude kinematics of a rigid body are described by the time derivative of the attitude quaternion. Some authors (Bloesch et al., 2016;Hertzberg et al., 2013) emphasize the manifold structure of attitude dynamics by defining q ∈ R 3 as a member of the associated Lie algebra, while other authors (Markley, 2003;Trawny and Roumeliotis, 2005) assume a first-order approximation such that q ∈ R 4 . In this article, while we use (22) to propagate along the manifold S 3 , we use the first-order approximation of the quaternion dynamics over a finite timestep t for computing the firstorder Jacobian matrices required by the extended Kalman filter (EKF). The first-order Taylor series approximation of ( 22), which assumes 1 2 ω 0 t is small, 1 yields the same result as (25). Using (25a), the quaternion time derivative for iRN is computed as
A similar analysis follows for bRN using (25b). In summary, attitude quaternion kinematics are represented by
Because unit quaternions do not form a vector space, quaternion error cannot be computed using vector subtraction. Rather, a true quaternion state q is represented as the quaternion multiplication of an estimated quaternion q and quaternion attitude error δq. By varying the order and direction of the quaternion multiplication, there are four possible methods to define attitude error:
Method 1: q q ⊗ δq, (
Method 2: q δq ⊗ q, (28b) Method 3: q q ⊗ δq -1 , Method 4: q δq -1 ⊗ q.
In this article, we use (28a) for iRN described in Section 6 and use (28b) for bRN described in Section 7: iRN: q q ⊗ δq, (29a) bRN: q δq ⊗ q.
(29b)
While using different definitions requires additional care when deriving the filter, ultimately this minimizes differences between the dynamics, measurement models, and keyframe reset steps of iRN and bRN. Figure 3
bRN: δq = q ⊗ q-1 . (30b)
Using ( 12), we can express (29) as iRN: R(q) = R(δq) R( q) , (31a) bRN: R(q) = R( q) R(δq) .
(31b)
When representing the attitude uncertainty associated with a quaternion error, a minimal representation is required. A quaternion is parameterized with four numbers, but only three are required to fully parameterize an orientation since orientations are associated with unit quaternions, elements of the three-dimensional group S 3 . Because the group is three-dimensional, the tangent space at the identity element, or Lie algebra, will be isomorphic to R 3 , and error covariances can be defined in this three-dimensional vector space. Accordingly, we represent the uncertainty in δq as the covariance of the vector δθ ∈ R 3 . The ∧ and ∨ operators of ( 18) and ( 19) define the mapping between the error quaternion δq and its minimal representation δθ as
By substituting (32) into ( 9) and ignoring second-order terms, it follows that
Aircraft attitude is commonly represented using three angles: roll φ, pitch θ, and yaw ψ. Yaw represents the rotation about the inertial z-axis (down). Pitch represents the rotation about the resulting y-axis. Roll represents the rotation about the x-axis formed after pitching and yawing. This sequence of rotations, known as 3-2-1 Euler angles, relates the vehicle's body frame to an inertial frame, and can be represented as the multiplication of three rotation matrices
where
Because quaternions are generally less intuitive, they are often mapped into roll, pitch, and yaw angles for plotting, analysis, and control. Expanding (35) using (9) and comparing terms, we obtain φ = atan 2q 0 q x + 2q y q z q 2 zq 2 xq 2 y + q 2 0 , θ = asin 2q 0 q y -2q x q z ψ = atan 2q 0 q z + 2q x q y q 2
xq 2 yq 2 z + q 2 0 .
(37)
To map Euler angles into a quaternion, we compose the attitude quaternion from the roll, pitch, and yaw components using the order derived in (11) such that
Note the order of composition is the opposite of ( 35) as described by ( 12). From ( 14), we obtain
(39) To show that ( 38) and ( 35) are consistent using the definitions in (39), we need only apply (9) and double-angle trigonometry identities to (39). For example, letting k 0 0 1 T , we see that
By substituting ( 39) into ( 38), a unit quaternion can be constructed from roll, pitch, and yaw angles as
Appendix A presents the derivation of the Jacobian that relates errors in the Euler angle decomposition to the attitude error state. Defining
the covariance of is related to the covariance of δθ as
where N = ∂ /∂δθ is given for the iRN case by (100) in Appendix A.
This section presents several properties of error quaternions that are needed in the derivation of the Jacobians used in the RMEKF. The first of these properties relates to the product of two error quaternions. Using ( 32) and (3a), and by dropping second-order terms, we obtain
from which we can conclude that
It can be similarly shown that
For the next property, we recall from Section 3.2 that quaternion conjugation operating on an arbitrary quaternion rotates the basis of the vector portion of that quaternion. Using Equation ( 13) with (32), we see that
In addition, ( 40) and ( 41) can be combined to verify that, for example,
(42)
With the quaternion notation and properties established, we are prepared to outline the proposed estimation framework: a continuous-discrete, indirect, multiplicative extended Kalman filter (MEKF). For generality, in this section we derive the MEKF without defining the specific state, input, and measurement variables. This derivation will be made more concrete in Sections 6 and 7, where the actual implementations with specific variable definitions are presented. This section is predominantly a review of techniques found in the literature, but introduces notation that helps to clarify the derivation.
A Kalman filter provides the optimal, maximumlikelihood state estimate for a linear system under Gaussian noise. It recursively estimates the evolution of the system state x as a function of the current state estimate x = E[x], input u, and measurement z. A Kalman filter also maintains an estimate of the state uncertainty, represented by the covariance matrix P, typically defined as
The EKF is an extension of the Kalman filter for systems with nonlinear dynamics and/or nonlinear measurement models. The EKF linearizes the system about the current maximum-likelihood state estimate. While optimality and convergence are no longer guaranteed as opposed to a linear Kalman filter, EKFs are widely used in practice for their robust performance and straightforward implementation. If the state x includes a quaternion, however, Equation ( 43) is fundamentally flawed. First, quaternion subtraction is not well-defined as described in Section 3.6, and second, Equation ( 43) is never full rank because quaternions are not a minimal representation. These issues are addressed using an error-state, or indirect, formulation of the Kalman filter.
The indirect Kalman filter tracks the error state δx and its uncertainty. Unlike the state x, the error state is defined as an element of a vector space by using a minimal attitude representation. The error state is a measure of the discrepancy between the true state x and a nominal state x nom , where x nom can be defined in a number of different ways, as described by Farrell (2008). When the system dynamics are especially well-modeled, such as for a spacecraft in orbit, x nom may be a predetermined feedforward state estimate. More commonly, the nominal state is the maximum likelihood state estimate x nom = E [x]. In this case, measurements provide feedback to update the nominal state, forcing the expected value of the error state to zero. Some indirect Kalman filter implementations differentiate between fast and slow measurements, and only update the nominal state for the slow measurements (Farrell, 2008;Maybeck, 1979). For such systems, x nom = E [x] at the fast rate, so that the expected error state is non-zero and must be propagated. For the derivation in this article, the nominal state is updated equivalently for every measurement, ensuring that x nom = x = E[x] at any given time.
Let x v and x q represent vector and quaternion portions of the state x, and δx v and δx θ be the corresponding elements of the error-state δx. We define these error-state elements as
where δq is defined by (30) using x q and xq . This results in the property
as derived in Appendix B.1. Because the error state is part of a vector space, and as a result of ( 45), the indirect Kalman filter represents state uncertainty with the well-defined covariance
As a note on interpretation, in this formulation the unknown true state is modeled as a random variable centered around the current state estimate. More specifically, the vector and quaternion portions of the true state are modeled as random variables using the inverse of (44) as
where δx v and δx θ are elements of the Gaussian random vector δx ∼ N (0, P) .
Kalman filters are decomposed into two steps: the propagation step and the update step, which are described in Sections 4.1 and 4.2. Section 4.3 discusses a method to handle delayed or out-of-order measurements. In general, the MEKF can use either an inertial or body-fixed coordinate frame. Several nuanced differences exist, however, and are highlighted as specific to iRN or bRN.
Consider the continuous-time system
where υ ∼ N (0, Q u ) and η ∼ N (0, Q x ) are zero-mean Gaussian random variables. More specifically, we assume that η and υ are uncorrelated,
and that the input and process noise are not correlated in time
where δ( t) is the Dirac delta function.
In the propagation step, the filter propagates the state estimates forward according to the nominal propagation dynamics as
When x is part of a vector space, the Jacobians needed for propagating the covariance are obtained from the first-order Taylor series expansion of the error-state dynamics as
When the state x includes quaternion terms, however, the error-state dynamics cannot be formed by simple subtraction as in (51a). In fact, as described in Section 3.6, the error state δx is of lower dimension than x. Noting that x is a function of x and δx according to (44), the dynamics of the error state can be expressed generally as a single function
where only δx, υ, and η are stochastic variables. 2 The function f then handles the quaternion portions of the state appropriately. We show in Appendix B.2 that
x, u) = 0. The Jacobians for the covariance propagation are computed from the first-order Taylor series expansion of ( 52) about
where 53) has the form of (51b), but the Jacobian terms differentiate the error-state dynamics with respect to the error state and input noise, rather than with respect to the state and input.
The error-state covariance propagation is given by differentiating (46) with respect to time and utilizing the linearized error dynamics from (53) as
To simplify terms, we solve the differential equation in ( 53) with initial conditions δx 0 to obtain
Using ( 55) and the properties ( 48) and ( 49), we see that
where the 1 2 is because the bounds of integration only use half of the area inside of the delta function. Similarly,
Because Q u and Q x are symmetric, combining ( 54), (56), and (57) we have that P evolves between measurements as
In summary, during the propagation step, x is propagated forward using (50) and the error covariance is propagated forward using (58). In addition, because the evolution of δx is given by ( 53) and E[δx]( 0) = 0, we have that E[δx]( t) = 0 over the propagation window.
For the update step, consider the measurement
where η z ∼ N (0, R) represents measurement noise. This measurement noise is usually additive if the measurement is a vector quantity, but if the measured quantity is a quaternion the noise is applied through quaternion multiplication.
The residual r is the discrepancy between the true measurement and the predicted measurement ẑ = h(x, 0) . Conventionally, a vector-space measurement is assumed, such that (59) is simplified as
In this case, the residual is found by subtraction,
and is modeled as
Jacobians are then computed using the first-order Taylorseries expansion as
These measurement and residual models share similar shortcomings with the conventional error-state dynamics presented in (51), namely they do not hold for quaternion measurements and (60) assumes the state x is of the same dimensionality as δx.
These issues can be addressed by expressing the residual model as a single function, paralleling the approach taken for the propagation step in Section 4.1. Again noting that x is a function of x and δx, the residual is modeled as a function r = h(δx, η z , x) , where only the first two variables are stochastic. For measurements z v of vector values, the residual value is computed by subtraction as
and is modeled as
where x can be rewritten in terms of δx and x according to (29) and by rearranging (44a). For measurements z q of quaternion values, the residual value is computed as the three-vector minimal representation of the error between the observed and expected quaternions according to (30) as
and is modeled as
where again x can be rewritten in terms of δx and x according to (29) and (44a).
It follows from ( 62) and ( 64) that the residual models have the property h(E[δx], E[η z ], x) = 0. The measurement models in this article are chosen so that the noise is additive in the residual space, implying that ∂r ∂η z = I. The measurement Jacobians are computed using the first-order Taylor-series expansion of
where
and
Assuming the measurement noise, error state, and input noise are uncorrelated, the residual uncertainty is
The Kalman gain uses the residual and state uncertainty to find the extent to which the residual should be trusted and applied. Using the residual covariance, the Kalman gain is
In fusing the information provided by the measurement, the a posteriori estimate of the error state, denoted with a + , is
With the additional information provided by the measurement update, the error state is no longer zero mean, violating the property in (45). Let x v and x q again be vector and quaternion states within x, and let v and θ be the corresponding portions of the Kalman update Kr. To ensure the error state remains zero mean, the Kalman update Kr is used to adjust x as
where ( 67a) and ( 67b) are specific to the quaternion errorstate definition used.
Finally, the covariance is updated conventionally as
In practice, we use the Joseph-form Kalman update,
because it improves numerical stability and ensures that the covariance matrix remains symmetric (Bar-Shalom et al., 2002). In summary, during the update step the measurement residual (61) or (63) provides additional information causing a non-zero E [δx]. Using the Kalman gain (66), x is updated according to (67) to ensure E[δx] remains zero mean. The error-state covariance is updated according to (68).
The Kalman filter assumes the state evolves according to a Markov process. As such, state estimates for all previous time steps are marginalized. This makes it difficult to compute a residual for delayed or out-of-order measurements when they finally arrive. In practice, delayed measurements are common. For example, visual odometry algorithms may require hundreds of milliseconds to perform the computer vision operations necessary to compute a measurement. Ideally this measurement is used to constrain the state of the vehicle when the image was taken, not to constrain the state of the vehicle when the measurement arrives.
To address delayed measurements, we use priority queues to save the last T seconds of inputs, measurements, states, and error-state covariances ordered by time. If a delayed measurement arrives with a time stamp more than T seconds old, we discard the measurement. Otherwise, we discard all saved states and error-state covariances that have a time stamp later than the time stamp of the incoming delayed measurement. At this point, we are left with the state and covariance estimate at the instant the delayed measurement should have arrived. We apply the measurement normally, and then use the input and measurement queues to re-propagate the MEKF to the current time instance. This approach provides the same state estimate as if all measurements had arrived at the correct time. Handling delayed messages in this way may not be practical for all processors. Similar methods are described by Bopardikar et al. (2013) and Shen et al. (2014).
Section 4 provided a general overview of the indirect MEKF. This section describes how the MEKF is adapted to the relative navigation framework.
Conventional filtering approaches directly estimate the vehicle's global state with respect to some inertially fixed origin, such as the GPS origin or the vehicle's starting location; however, when only relative position measurements such as those obtained from visual odometry or laser scan matching are available, the vehicle's global position and heading are unobservable (Jones et al., 2007;Martinelli, 2012;Weiss et al., 2012). Over time, directly estimating these unobservable states leads to inconsistent and unbounded state uncertainties, which can degrade accuracy and cause irregular state jumps in the filter (Bailey and Durrant-Whyte, 2006;Julier and Uhlmann, 2001;Kottas et al., 2013). Methods for mitigating these issues have been proposed (Bailey and Durrant-Whyte, 2006;Castellanos et al., 2007;Kottas et al., 2013), but the core underlying issue of unobservable states can be avoided entirely by reformulating the problem in terms of relative states (Wheeler et al., 2018).
The relative navigation approach maintains observability of the filter states by estimating the pose of the vehicle with respect to a local coordinate frame referred to as the node frame. This node frame is positioned at zero altitude directly below the most recent odometry keyframe, but is gravity-aligned (i.e., the heading is aligned with the vehicle's heading when the keyframe was declared, but there is no pitch or roll). As a result, the odometry provides nearly direct measurements of the position and heading of the vehicle with respect to the current node frame, making those states observable by construction.
Owing to the way the node frame is defined, the roll and pitch components of the vehicle's attitude (φ, θ ), as well as the vehicle's altitude p z , are estimated as if they were defined with respect to a global origin. These states are not affected when transitioning from one node frame to another and so are, in effect, independent of the current node frame. On the other hand, the horizontal position and heading states (p x , p y , and ψ) define how the vehicle has moved since the last node frame, and are termed relative states. Each time a new keyframe is declared, a new node frame is also declared and the relative states (p x , p y , and ψ) are reset to zero. The covariances associated with the relative states are also reset to zero, since the vehicle is, by definition, at the location of the node frame and so there is no uncertainty in these states. This is illustrated in Figure 4. The non-relative states (roll, pitch, altitude, body-fixed velocities, and body-fixed inertial measurement unit (IMU) biases) and their covariances are unchanged by the keyframe reset operation. Note that resetting the heading component of the state and covariance is non-trivial when attitude is parameterized with a quaternion. Prior to the reset, the vehicle's current relative pose estimate and covariance are passed to a back-end pose-graph map that concatenates the relative poses into an estimate of the vehicle's global path and current global pose (Wheeler et al. 2018).
We describe the keyframe reset operation mathematically as follows. Let n(x) define the keyframe reset operation. The estimated state is reset as
The error state after the reset, δx + , is the difference between x + and x+ as defined by ( 44). Again recalling that since x is a function of δx and x, we can express the error state after the reset as a single function
This allows the covariance to be updated as
where
.
The details of the reset operation n(x) and the Jacobian N are presented in Section 6.3 for iRN and in Section 7.3 for bRN.
This formulation provides several advantages in terms of estimator performance. One of these advantages is that when the covariance associated with the relative states is reset to zero, uncertainty is in essence removed from the filter and delegated to the back-end map, which helps to maintain filter consistency (Barfoot and Furgale, 2014). As a result, the covariance in the filter also remains bounded. In addition, because the distances between keyframes are relatively small, the state error remains small, avoiding significant linearization errors that can cause inconsistency in a global estimator (Castellanos et al., 2007).
While reconstructing the global pose estimate requires implementing a back-end pose-graph map, this architecture has also been shown to improve consistency and accuracy of the final global pose estimate over global filter approaches, even in the absence of additional pose-graph constraints such as loop closures (Wheeler et al., 2018). It also has the advantage of avoiding large, potentially destabilizing state jumps when new global information becomes available. The requirement to implement this pose-graph map is also not a particularly onerous one, seeing as some global filter approaches already use a back-end batch-processed map to provide updates to their global filter (Shen et al., 2014;Weiss and Siegwart, 2011).
The various steps for implementing an RMEKF are summarized in Algorithm 1, along with references to the key equations. The specific implementation equations are derived in Section 6 for iRN and in Section 7 for body-fixed robocentric relative navigation. Reset state using (69) 16:
Reset uncertainty using (70)
iRN estimates the vehicle's position and attitude with respect to the current node frame. While the current node frame changes regularly, each is gravity-aligned and defined inertially. For this reason, typical UAS dynamics from the GPS/INS literature are applicable. Section 6.1 outlines the input, state, and dynamics for the system, including the error-state dynamics. Section 6.2 defines the measurement models and Section 6.3 outlines the keyframe reset step. In general, the derivations in this section improve upon the mathematical rigor of the derivations in prior work (Leishman and McLain, 2014) and provide some corrections. New contributions that extend that work are noted in their respective sections.
This section derives the state propagation model used in the filter. Section 6.1.1 defines the state vector. Section 6.1.2 discusses how measurements from an IMU are incorporated into the propagation model, and Section 6.1.3 defines the state propagation model and derives the associated Jacobians.
Vectors use a forward-right-down coordinate frame, with axes labeled x, y, and z. In this article, the position vector c p b a denotes the position of frame b with respect to frame a, expressed in frame c. Unless otherwise noted, position vectors are expressed in the originating frame, that is, frames a and c are the same. When this is the case, the prescript is usually omitted for brevity, so that p b a a p b a . However, the prescript is occasionally included for clarity.
iRN estimates the position and attitude of the vehicle's body frame b with respect to and expressed in the current node frame n, denoted by the pose ( p b n , q b n ). Let frame k represent the vehicle's body frame at the instant in time that the current keyframe image was taken. The estimator tracks the pose of k with respect to and expressed in n, denoted as ( p k n , q k n ), which is an extension to prior work (Leishman and McLain, 2014). As illustrated in Figure 2, the keyframe states ( p k n , q k n ) include the altitude, roll, and pitch of the vehicle at the moment a keyframe is declared. As the vehicle state is not a vector when it contains quaternion elements, we define the state as the tuple I define the true body-fixed angular rates and accelerations as measured by an ideal IMU, respectively. For brevity, we omit specifying the frames and write ω and a throughout the article.
An IMU provides measurements ω and ã that are corrupted by unknown biases β ω and β a and zero-mean Gaussian noise processes υ ω and υ a , such that the measured values could be modeled as
Recognizing that the actual measured values ω and ã are fixed at a given timestep and that the noise terms are considered to be uncorrelated in time, the unknown true values are modeled as random variables as
where ω, a, β ω , β a , υ ω , and υ a are random variables and ω and ã are considered as constants. The nominal value of the angular rate used for propagation is then the expected value of the true angular rate
and the nominal value of the acceleration used for propagation is the expected value of the true acceleration
IMU data can be treated either as a system input or as a measurement, in a trade-off based on IMU quality and model accuracy. When a reliable vehicle model is available, the controller's output (e.g., motor commands) can be used as an input to the observer to propagate the state forward. In this case, IMU data are incorporated as measurement updates providing feedback. This approach leverages the most information, but requires careful characterization of the vehicle's dynamics. Another common approach, known as mechanization, treats the IMU measurements directly as inputs to the filter dynamics, which replaces the vehiclespecific dynamics with kinematic equations. This simplifies the propagation dynamics and eliminates sensitivity to modeling errors, but does not use any information about how the vehicle behaves.
For the filter design in this article, the angular velocity measured by the rate gyros ω is treated as an input to the propagation equations. Following Leishman et al. (2014a), the z component of the accelerometer measurement is also treated as an input, while the x and y components are used as measurement updates. The following paragraphs explain the derivation and justification for this approach.
Using Newton's second law, the velocity dynamics can be modeled as
where m is the mass of the vehicle and B F are the forces acting on the vehicle expressed in the body frame. As illustrated in Figure 6, the principal forces that act on a multirotor are gravity B F G = mR(q b n ) g, thrust B F T = Tk, and the simplified drag force B F D = -μ 0 v, where k 0 0 1 T , T is the total rotor thrust, μ 0 is the nominal drag coefficient, and g = gk with g being the standard acceleration due to gravity. Substituting these forces into (75
While technically correct, the dynamics in ( 76) are challenging to use in practice because the thrust T is difficult to model and so is generally unknown. This difficulty is addressed through mechanization by utilizing the accelerometer measurements directly in the propagation model. As explained by Leishman et al. (2014a), accelerometers measure the specific force (not including gravity) expressed in the body frame, so that
Substituting ( 77) into ( 76) yields the common mechanization dynamics
Equation ( 78) eliminates the need for modeling complicated vehicle dynamics, but ignores information about the dynamics that might improve estimator performance by building up cross-correlation terms in the covariance matrix P.
In practice, we have found the most success using a combination of ( 76) and (78). Noting that the unknown thrust term in (76) appears only in the z component, a hybrid propagation model is obtained by substituting only the z component of ( 77) into (76) to obtain
where a z = k T a, μ μ 0 /m is the specific drag coefficient, and k Ikk T is a projection matrix that projects onto the plane normal to the k-axis.
As a result of this hybrid approach, we consider the gyroscope and z-axis accelerometer measurements as system inputs, while using the horizontal accelerometer measurements as feedback in the update step. The system input and input noise are therefore defined as
where ãz = k T ã and υ a z = k T υ a .
6.1.3. Propagation model. The system dynamics are modeled as
where ω is given by (72a), a z is given by the z component of (72b), and η v , η β ω , and η β a are zero-mean Gaussian noise processes for the corresponding states. The state p b n is propagated according to a standard kinematic model, q b n is propagated according to (26) and the discussion in Section 3.4, and v is propagated according to (79). The dynamics for the bias states β ω and β a are modeled as random walks, while the keyframe states p k n and q k n represent the relative pose of static coordinate frames and therefore do not change. The drag term μ is constant and so has zero dynamics, but uncertainty in this state is considered by assigning it a non-zero initial covariance.
The state estimate is propagated by substituting the expected values of the state, input, and process noise terms into (80) as
where ω is given by ( 73) and âz is given by the z component of ( 74). The error-state dynamics are found by relating ( 80) and (81) using the error-state definition (44). The first-order approximation of the error-state dynamics are
and are derived in Appendix C. Differentiating the error state dynamics with respect to the error state and input noise results in the following propagation Jacobians:
The accelerometer, altimeter, and visual odometry or laser scan matching algorithm provide measurements to constrain state estimates. For each sensor, the measurement model, residual model, and residual Jacobians are defined.
Because the z portion of the accelerometer measurement is used as an input to the propagation, the update step uses only the x and y components of ã, such that z acc = I 2×3 ã.
The accelerometer measurement model can be derived from (71a) and (77) as
where the thrust term T m k drops out because I 2×3 T m k = 0. We can expand this model in terms of δx and x according to (44) and drop second-order terms 3 to obtain h acc ( x, u+υ)=I 2×3 -μ+δμ v+δv + βa +δβ a +υ a ≈I 2×3 -μv-μδv-δμv+ βa +δβ a +υ a .
From (82), the estimated measurement is
For a given acceleration measurement z acc , the residual is r acc = z acch acc ( x, u) , and is modeled as
The measurement Jacobian is therefore
The measurement noise is the x and y components of the accelerometer noise,
6.2.2. Altimeter. The altimeter model is for an ultrasonic range finder, which reports the nearest return in its conical field of view. As a result, the sensor reports height above ground regardless of the current attitude of the vehicle, as long as roll and pitch angles are moderate. The measurement model and its estimate are
For a given altimeter measurement z alt , the residual is
which is modeled as
resulting in the measurement Jacobian H alt = -k T 0 0 0 0 0 0 0 .
Incorporating measurements from visual odometry algorithms is somewhat more involved than the previous measurement models. Figure 7 outlines the relationship between the visual odometry output and the state. The visual odometry output ( p c kc , q c kc ) relates the current camera frame to the keyframe camera frame. The transform ( p c b , q c b ) express the pose of the camera frame with respect to the vehicle's center of mass. Commonly the camera frame used in visual odometry algorithms is derived from the image plane such that the camera frame's z-axis references forward motion (depth). In this case, q c b encodes the mapping from the body frame's forward-right-down coordinate frame to the coordinate frame used by the camera. The transform ( p c b , q c b ) can be found through offline calibration or online as described by kc , q c kc ) is the output of the visual odometry algorithm. The transform ( p c b , q c b ) defines the pose of the camera frame with respect to the vehicle's center of mass, and is assumed to be fixed and known. The frame k represents the pose of the body at the time of the keyframe, and the frame kc represents the pose of the camera at that time. As a result, p kc k = p c b and q kc k = q c b . Leishman and McLain (2014). In this article, we assume that the camera is rigidly mounted to the body and ( p c b , q c b ) is static and known. In addition, because ( p kc k , q kc k ) also represents the transform between the body and camera, but at the time of the keyframe, p kc k = p c b and q kc k = q c b . Figure 7 can be used to informally understand how defining a relative state improves observability. The pose ( p k n , q k n ) encodes the roll, pitch, and altitude of the vehicle when the keyframe is declared, all of which are observable using an altimeter and IMU. This fact, in connection with assuming ( p c b , q c b ) is known, ensures that the measurements ( p c kc , q c kc ) constrain the vehicle's current pose ( p b n , q b n ). Using Figure 7, the relative translation measurement is modeled as
Dropping prescripts and recalling that p kc k = p c b and q kc k = q c b , this becomes
We expand this model according to ( 44) and (31a), then use ( 33) and ( 34) to obtain
The measurement Jacobian is
The derivation of this Jacobian is a new contribution compared with prior work (Leishman and McLain, 2014).
The laser scan matcher measurement model is a novel contribution of this work. Laser scan matching algorithms, such as the iterative closest point (ICP) algorithm (Censi, 2008), compute the planar translation and heading change of a laser scanner between the keyframe and current scans. Because the out-of-plane motion of UAVs (roll and pitch) introduces distortions that are not modeled by the scan-matching algorithm, in practice laser scans are commonly tilt-compensated before they are passed to the scan matcher (Shen et al., 2011;Tomic et al., 2012). Tilt compensation orthorectifies the laser scan by projecting it onto a level horizontal plane, using the 2.5D world assumption that all scanned surfaces are vertically uniform. Scan returns from the floor or ceiling are also often removed during this process. The 2.5D assumption is commonly violated in the real world, but in urban environments this approach has been demonstrated to provide acceptable performance (Shen et al., 2011).
To perform tilt compensation on a laser scan, we first compute the quaternion q o c that rotates points expressed in the rolled and pitched laser frame c (including any roll or pitch in the body-to-sensor transform) to the orthorectified frame o as
where q θ b n and q φ b n are computed from q b n using Equations ( 37) and ( 39). Then for a laser scan S = {(r, θ )} represented as a set of range/bearing tuples (r, θ ), the tilt-compensated scan S is computed as
For the orthorectification of the keyframe scan, we note that because q k n = q θ b n ⊗ q φ b n (as shown in Section 6.3) and q kc k = q c b , the rotation q o c is equivalent to
The associated rotation matrix is given by
In the following discussion, we derive the translation and heading portions of the measurement model independently, but they can be combined into a single update by stacking the two measurement model vectors and Jacobian matrices.
The translation measurement model is constructed by expressing the visual odometry measurement model of Equation ( 84) in the orthorectified frame associated with the keyframe, and then extracting the x and y components:
We expand using ( 44), (31a), and (34) then drop secondorder terms to obtain
The estimated measurement model is
and the residual is
which is modeled as
The measurement Jacobian is
The derivation of the heading portion of the measurement model follows a similar strategy. Recalling from Section 3.2 that the quaternion conjugation operation can be used to express the vector portion of a quaternion in another coordinate frame, we construct the heading measurement model by first rotating the visual odometry rotation model from Equation ( 85) into the orthorectified frame:
The heading measurement model is then the yaw component of q lr : h lr (x) = ψ(q lr ) +η lr
The estimated measurement model is
where qlr = q b n ⊗( q k n ) -1 . The residual is given by r lr = z lrh lr (x) , which is modeled as
as in Appendix A. We can then compute the Jacobian of the residual using the chain rule as
where N lr is given by Equation ( 100) in Appendix A. We compute δθ lr using (30a), ( 29a), ( 40), and (41) as
which we take the partial derivatives of and substitute into (86) to obtain
When a new keyframe is established, the relative part of the state is reset, as described in Section 5. As shown in
As such, the error state reset is
The other vector portions of the state, including v, β ω , β a , and μ, do not change.
Resetting the yaw portion of the attitude states is slightly more complicated. Each new node frame is established such that the vehicle's yaw is identically zero. Setting ψ = 0 in (39), we see that
where φ and θ are computed from q b n using (37). The covariance update for the attitude error states is governed by
which is derived in Appendix E. When roll and pitch are approximately zero, we note that
In summary, the keyframe reset requires updating the state estimate according to (87) and ( 89) and updating the covariance according to (70) where
kk T 0 0 0 0 0 0 0 0 N θ 0 0 0 0 0 0 0 0 I 0 0 0 0 0 0 0 0 I 0 0 0 0 0 0 0 0 I 0 0 0 kk T 0 0 0 0 0 0 0 0 N θ 0 0 0 0 0 0 0 0 0 0 0 0 0 1
.
While defining the vehicle's pose with respect to an inertially defined origin uses more conventional dynamic equations and results in more intuitive state estimates, an increasing number of estimators use body-fixed or robocentric state definitions. In the body-fixed case, the position and attitude of an inertially fixed origin is estimated with respect to the current vehicle's pose. Body-fixed state definitions have been shown to improve filter consistency (Castellanos et al., 2007) and facilitate local guidance and control algorithms (Owen and Montano, 2006;Yu and Beard, 2013). For example, the obstacle avoidance and visual-servoing problems requires a vehicle to make navigation decisions after estimating the pose of objects with respect to its current pose. When an inertial representation of the state is desired, the body-fixed state and its covariance can be readily transformed into an inertial frame as described in Appendix G.
The relative estimator in the relative navigation architecture can be formulated using a body-fixed state definition. In this case, bRN estimates the state of the node frame with respect to the current body. As before, when a new keyframe is declared, the horizontal position and heading states are reset. The principal difference is that body-fixed dynamics are used and attitude error is defined differently.
Unless specified explicitly, the equations and definitions in Section 6 are also assumed for bRN. This section follows the same outline as Section 6, first describing the input, state, and dynamics for the system in Section 7.1, and then defining the measurement models and keyframe reset in Sections 7.2 and 7.3, respectively. The derivations in this section represent a novel contribution not contained in prior work.
bRN estimates the pose of the node frame n with respect to and expressed in the current body frame b, denoted as ( b p n b , q n b ). The states b p n b and n p b n represent the same displacement, but are pointed in opposite directions and are expressed in different frames. Specifically,
The states q n b and q b n are inverses of each other:
The estimator also tracks the pose of frame n with respect to and expressed in frame k, denoted as ( k p n k , q n k ) and shown in Figure 8. For bRN, the vehicle's state is the tuple
while the input remains unchanged from Section 6.1. The state dynamics are
where derivations for the position and attitude dynamics are found in Appendix D, and where ω is given by (72a) and a z is given by the z component of (72b). Note that (92b) only differs from (80c) by a single transpose.
For bRN, the error-state vector δx ∈ R 22 is defined as
The first-order approximation of the error-state dynamics, also derived in Appendix D, is
Differentiating the error-state dynamics with respect to the error state and input noise results in the following propagation Jacobians:
where ω is defined in (73).
The measurement models differ when using a body-fixed parameterization as outlined in the following.
Because the accelerometer model is independent of attitude and position, the model remains unchanged for bRN. See Section 6.2.1.
As described in Section 6.2.2, the sonar altimeter measures height above ground. By relating ( 90) and ( 83), the vehicle's altitude is
where the rotation expresses the height in an inertial frame.
The estimated measurement model is
For a given altimeter measurement z alt , the residual is
which is approximated using (31b), (44a), (33), and (4) as
resulting in the measurement Jacobian
p n b , 0, 0, 0, 0, 0, 0 .
Figure 8 presents the relative position and attitude measurement model for bRN. Note that the direction of ( p n b , q n b ), ( p n k , q n k ), and ( p kc c , q kc c ) are reversed when compared with Figure 7. A relative measurement can generally be measured either direction. Defining the measurement to point in the same direction as the state estimate avoids unnecessary coupling of the heading estimate into the translation measurement model, which in practice has shown to improve performance.
From Figure 8, the relative position update is
Dropping prescripts and recalling that p kc k = p c b and q kc k = q c b , this becomes
Expanding according to (44), (31b), (33), and (34) gives
Expanding and removing higher-order terms,
The estimated measurement model is
We then model the residual according to (62) as
resulting in the residual Jacobian
where
The relative rotation measurement model also follows from Figure 8. Specifically,
The estimated measurement model is
For a given relative attitude measurement z vor , the residual is
which is modeled and then simplified using ( 41) and ( 42) as
resulting in the measurement Jacobian
As with the iRN case, laser scans are orthorectified before being passed to the scan matcher. The quaternion q o c that describes the rotation from the laser frame c to the orthorectified frame o is given by
where q b n =(q n b ) -1 , q θ b n and q φ b n are computed from q b n using Equations (37), and q o b is defined as
The orthorectified laser scan S is then computed from the original laser scan S = {(r, θ )} as
We note that the rotation matrix associated with q o c can be written as
The translation portion of the measurement is modeled as the x and y component of visual odometry translation model in Section 7.2.3, after it has been rotated into the orthorectified frame:
The estimated measurement model is
The residual Jacobian is derived in Appendix F, and is given by
where
For the rotation portion of the measurement model, we first express the visual odometry rotation model in the orthorectified frame:
which is estimated as
The measurement model is then the yaw portion of q lr , h lr (x) = ψ(q lr ) +η lr , which is estimated as
The residual Jacobian is derived in Appendix F, and is given by
where
The keyframe reset step for bRN is somewhat less intuitive owing to the body-centric representation of position and orientation states, but can be derived by following the iRN reset step. Following ( 91) and ( 88),
, where φ and θ are computed from q b n =( q n b ) -1 using (37). Note that even though bRN expresses attitude in the body frame, the angles φ, θ , and ψ continue to represent conventional roll, pitch, and yaw Euler angles that express the orientation of a body with respect to an inertial frame. The Jacobian for the attitude reset is derived in Appendix E, and happens to be identical to that for the iRN attitude reset:
The derivation for the position reset is more involved, and is given in Appendix E. The resulting reset operation is
Because frames b and k are at the same location when the reset occurs, we also have
The total keyframe reset Jacobian, also derived in Appendix E, is given by
The RMEKF was implemented in C++ using the ROS (Quigley et al., 2009) framework. The following sections present simulation and hardware results for the performance of the RMEKF on a multirotor vehicle. The simulation results in Section 8.1 illustrate the performance of the estimator under ideal conditions, where all noise is Gaussian with known covariance and all biases are known. The hardware results in Section 8.2 demonstrate the performance of the estimator in real-world conditions, where the Gaussian noise assumption is not necessarily met and where covariance and biases are unknown. The results in this section illustrate the typical accuracy of the relative state estimates produced by the RMEKF. Since estimating relative states is a unique approach, direct comparison with related global estimation techniques in the literature cannot readily be accomplished unless the estimator in this article is combined with a simple global back end. For those comparisons, the reader is referred to the companion work by Wheeler et al. (2018). The more extensive flight-test results of Wheeler et al. (2020) additionally demonstrate the practical performance of the RMEKF for several extended flights of a multirotor platform, including closing control loops around the estimates.
The simulation provided accelerometer, gyro, altimeter, and visual odometry measurements corrupted by normally distributed noise. Slowly drifting biases were also added to the simulated IMU data. Sensor noise (υ ω , υ a , η vot , η vor , and η alt ) was sampled from normal distributions with the following standard deviations that are typical of low-cost hardware sensors: σ ω = 0.13 rad/s, σ a = 1.15 m/s 2 , σ vot = 0.02 m, σ vor = 0.01 rad, and σ alt = 0.01 m. New keyframes were established when the vehicle moved more than 0.2 m or yawed more than 20 • . The estimator was evaluated during various maneuvers ranging from mild to aggressive, where during the aggressive maneuvers the vehicle's speed exceeded 25 m/s and the bank angle exceeded 45 • .
Figures 9 and 10 show a 3 second snapshot of the performance of the iRN and bRN estimator at tracking the vehicle's pose, where the small time window was selected to make the relative state reset visible. During these 3 seconds, the vehicle was moving forward at nearly constant velocity while maintaining a nominal height above ground of 1.25 m, and while gradually slowing its clockwise yaw motion. The vertical gray lines indicate the time when a new keyframe is declared. While the state is defined using quaternions, Figure 9(b) uses ( 37) to plot roll, pitch, and yaw angles.
Figure 9 highlights several interesting practicalities of relative navigation. As discussed in Section 5, Figure 9 illustrates how the forward, right, and yaw states are reset to zero with each newly declared keyframe, while the altitude, roll, and pitch states remain continuous. It should be noted that while roll and pitch are continuous, discontinuities appear in each of the four quaternion states. Because the discontinuities in the relative states occur at known times, they are easily accounted for and so, in practice, do not cause problems with control stability. Figure 9 also illustrates that keyframes do not reset at fixed intervals, but rather reset based on how far the vehicle has traveled since the previous keyframe. Certain sensors, such as a laser scanner with a long range and wide field of view, facilitate longer distances between keyframes.
The RMEKF performed very similarly when using the body-fixed dynamics presented in Section 7. Figure 10 shows the performance of the bRN estimator for position states over the same window of time as Figure 9. The attitude performance of bRN (not shown) is nearly identical to the attitude performance of iRN with the exception of a change in sign for the angles. Note in Figure 10 that each of the position states experiences slight discontinuities at the keyframe resets and that the position estimates do not reset to zero. This is because the keyframe reset step removes all horizontal translation from the state but continues to track the vehicle's height above ground. As the bRN position state is expressed with respect to the rolled and pitched body frame, some of the height above ground is mapped into the forward and right components. When the bRN state estimates are expressed with respect to the current node frame using ( 90) and (91), they are nearly identical to the estimates shown in Figure 9.
The vehicle used for hardware results, a hexacopter in a Y6 configuration, is shown in Figure 11. Attitude control is performed by a Naze32 autopilot running the ROSflight 4 firmware. IMU data is streamed from the low-cost MEMS IMU on the autopilot (an InvenSense MPU-6050) at 500 Hz. Altimeter data is obtained from a MaxBotix LV-MaxSonar-EZ3 ultrasonic range finder. All estimation is performed in real-time on the onboard computer with an Intel i7 processor. Visual odometry is also computed on the onboard computer at 15 Hz from a forward-facing Asus Xtion Pro Live RGB-D camera, using the depth-enhanced monocular odometry algorithm (Zhang et al., 2014) modified to output relative measurements. New keyframes were established when the vehicle moved more than 0.5 m or yawed more than 20 • , although more sophisticated keyframe-selection algorithms could be implemented. A constant, typical value was chosen for the visual odometry measurement covariance, but future implementations could take advantage of algorithms that estimate the covariance from frame to frame (e.g., Anderson et al., 2019).
For the comparisons in this article, truth reference was provided by a motion capture system. The positioning accuracy of the motion capture system is on the order of 0.5 cm or better, and the attitude accuracy is on the order of 0.1 • . A calibration routine was used to register the quadrotor body frame to the motion-capture frame. To compare the relative state estimates with the global motion-capture data, the reference frames were synchronized by applying the same position and orientation reset steps described in Sections 6.3 and 7.3 to the motion-capture data each time a visual-odometry keyframe reset occurred. While there are small timing delays and other error sources inherent in this approach, we believe this "truth" reference to be highly representative of the true motion and an appropriate reference source for system evaluation.
Figure 12 shows the estimator performance for the RMEKF using inertial dynamics (iRN). A typical 10second window of the flight is shown to make the relative state reset visible. The estimates track the truth well. Again, note that the forward and right position estimates in 1.
The performance of the estimator using body-fixed dynamics (bRN) was very similar to the inertial dynamics. Plots are not shown for the bRN results, but the RMS errors for a comparable flight test are summarized in Table 1.
Looking more closely at Figures 12(a) and (b), it appears as if the relative states do not reset all the way to zero. The reason for this is the delay in the visual odometry processing, and is illustrated in Figure 13. On average, it takes approximately 115 ms from the time an image is captured (the dashed vertical line in Figure 13) to the time the visual odometry message is published (the solid vertical line). However, the estimator uses the timestamp for when the image was captured when applying the odometry measurement, as described in Section 4.3. When a new keyframe image is captured, during the time window between the image capture and odometry message the estimator continues operating with respect to the previous keyframe. Once the new keyframe odometry message arrives, the estimator rewinds to the time of the image capture, resets the state to zero, then re-propagates the IMU and altimeter messages that were queued up to the visual odometry message time. As a result, the first message that the estimator publishes with respect to the new keyframe includes the 115 ms of IMU re-propagation and is non-zero. However, the relative state was actually reset to zero at the time of the image capture.
While the global state of a UAS is not observable when navigating through GPS-denied environments, it is possible to maintain observability by subtly restructuring the problem using the relative navigation framework. Previous simulation studies show strong theoretical motivations for the relative navigation framework, including improved consistency, bounded covariance, and improved accuracy (Wheeler et al., 2018). In addition, significant hardware results have demonstrated the efficacy and practicality of relative navigation for small UASs when using a relative MEKF for state estimation (Wheeler et al., 2020). This article rigorously derives the RMEKF used in Wheeler et al. (2018) and Wheeler et al. (2020), presenting the mathematics necessary to apply relative navigation to the UAS state estimation problem. Its tutorial nature and step-by-step derivations make this article a self-contained resource for extending the approach to other applications.
By defining the state with respect to a local coordinate frame, this article demonstrates how to leverage relative measurements from a visual odometry algorithm to ensure an observable state. New visual odometry and laser scan-matching measurement models are proposed, and a unique keyframe reset step is presented to ensure filter states are fully observable even when global information is not available.
The RMEKF is demonstrated in simulation and hardware to work effectively for both inertially defined and body-fixed vehicle dynamics to produce accurate state estimates with bounded uncertainty. While the inertial and body-fixed definitions yield similar results, this article explicitly outlines the differences that arise in the state estimator, including differences in the error state definition, measurement models, quaternion integration procedure, and keyframe reset step. Additional contributions of this article include a tutorial introduction to indirect MEKF, an exposition of Hamilton quaternions, and the derivation of several novel properties of error quaternions necessary for partially updating a quaternion and its covariance.
we wish to derive an expression for the Jacobian N ∂ ∂δθ .
The derivation and expressions that follow show some similarity to those presented by Barfoot et al. (2011), but are derived in the context of an error-state formulation. We begin by deriving expressions for the error quaternions for the yaw, pitch, and roll quaternions of Equations ( 38) and (39). For example, with the iRN definition of the error quaternion, we can compute δq ψ from (30a) and ( 39) as
where the final step uses the standard angle-difference trigonometric identities. Following a similar analysis, we see that (94) also holds for bRN using (30b). We likewise see that
for both the iRN and bRN attitude error definitions. Because these errors are all expressed in different intermediate frames of the Euler rotation sequence, δq = δq ψ ⊗ δq θ ⊗ δq φ in general.
Assuming small attitude errors, we approximate ( 94) and ( 95) with the first-order Taylor series as
These error states are represented minimally according to (19) as
(96) We then define
Again, it should be noted that δθ = in general. We now wish to solve for δθ in terms δθ ψ , δθ θ , and δθ φ . For the iRN case, starting with (30a) and using (38), we see that
We can expand (97) using ( 36) and ( 96) to express δθ in terms of as
The determinant of the matrix in (98) equals cos θ . Therefore, assuming that θ = ± π 2 , we solve (98) for as
from which we obtain
A similar procedure could be used to derive the Jacobian N for the bRN case. However, the definition and interpretation of roll, pitch, and yaw are less obvious for the bRN case, so we will omit the derivation of N here and instead derive the relevant expressions in the text as they arise.
In this appendix, we show that the expected error state remains zero mean. Specifically, we show that the expected value of the error state is zero, as indicated in (45), and that the error-state dynamics are trivial when the estimation error and input noise are both zero.
Property (45) stems from the linearity of the expectation and quaternion multiplication, inverse, and ∨ For the following derivations, recall that x is modeled as a random variable, while x is a constant value for a given timestep. Given (44a) and x = E [x], for vector portions of the state we have
Using (30a) as the attitude error definition for (44b), for quaternion portions of the state we have
A similar derivation is also possible for the attitude error definition in (30b).
For vector portions of the state, from (44a), (47), and (50) we have δ ẋv = ẋv -ẋv = f v (x, u + υ) +η vf v (x, u) fv (δx, υ, x, u) +η v .
We note that x can be rewritten in terms of δx and x using For quaternion portions of the state, for iRN we start with (29a):
x q = xq ⊗( δx θ ) ∧ =⇒ ẋq = ẋq ⊗( δx θ ) ∧ +x q ⊗( δ ẋθ ) ∧ .
Multiplying on the left by (x q ) -1 gives (x q ) -1 ⊗ ẋq =(x q ) -1 ⊗ ẋq ⊗( δx θ ) ∧ +( δ ẋθ ) ∧ =⇒ δ ẋθ = (x q ) -1 ⊗ ẋq -(x q ) -1 ⊗ ẋq ⊗( δx θ ) ∧ ∨ = (x q ) -1 ⊗ f q (x, u + υ) -(x q ) -1 ⊗ f q (x, u) ⊗( δx θ ) ∧ ∨ f(δx, υ, x, u) , where again x can be rewritten in terms of δx and x using (29a) and (44a). Note that we have omitted the process noise η since the quaternion propagation dynamics in this article are exact kinematic expressions. Evaluating fq at E[δx], E[υ], x, u gives fq (E[δx], E[υ], x, u) = (x q ) -1 ⊗ f q (x, u) -(x q ) -1 ⊗ f q (x, u) ⊗0 ∧ ∨ = 0.
A similar analysis can be carried out with the bRN attitude error definition in (29b).
Multiplying both sides by two, dropping the scalar term, and ignoring higher-order terms yields
The only change in the velocity error dynamics is the gravity term, so that
.
During the keyframe reset step, introduced in Section 5 and detailed in Sections 6.3 and 7.3, the relative states and their associated covariance are reset to zero. For the position states, only the altitude of the vehicle is kept, while for the attitude states the uncertainty associated with yaw is removed from the filter while the uncertainty associated with roll and pitch is maintained. Appendices E.1 and E. (104) for both iRN and bRN attitude definitions. Appendix E.3 derives the position reset and its Jacobian for bRN.
Recall from (99) in Appendix A that the Euler angle errors are related to the attitude error state δθ as The true and estimated attitude states after the keyframe reset step consist of the roll and pitch components of the attitude prior to the reset,
The resulting attitude error is obtained from (30a) and (29a) as δq b n + =( q b n + ) -1 ⊗ q b n + = q-1 φ ⊗ q-1 θ ⊗ q θ ⊗ q φ = q-1 φ ⊗ q-1 θ ⊗ qθ ⊗ δq θ ⊗ qφ ⊗ δq φ = q-1 φ ⊗ δq θ ⊗ qφ ⊗ δq φ , which, similar to (42), implies that
Continuing the notation presented in Section 7, we describe bRN attitude error with δq n b and δθ n b . The typical 3-2-1 Euler angles assume an inertial attitude. To express a body-fixed attitude while still maintaining the intuitive roll, pitch, and yaw rotations, the order and sign of the rotation sequence must be flipped as
Expanding using the error state definition (30b),
which is approximated as
Combining ( 108) and ( 109) using (30b) gives (113) which confirms (104) for bRN. Note that the values of φ and θ used to evaluate N θ are obtained from the Euler decomposition of ( q n b ) -1 .
Following (90) we can write by rearranging (90). We note from ( 91) and (38) that
=⇒ R(q n b ) = R T (q ψ ) R T (q θ ) R T (q φ ) , so (114) becomes
We next observe that kk T R T (q ψ ) = = -kk T R T (q θ ) R T (q φ ) p n b . Noting that The keyframe pose ( p n k , q n k ) is transformed similarly, yielding