Relative multiplicative extended Kalman filter for observable GPS-denied navigation

Abstract

This work presents a multiplicative extended Kalman filter (MEKF) for estimating the relative state of a multirotor vehicle operating in a GPS-denied environment. The filter fuses data from an inertial measurement unit and altimeter with relative-pose updates from a keyframe-based visual odometry or laser scan-matching algorithm. Because the global position and heading states of the vehicle are unobservable in the absence of global measurements such as GPS, the filter in this article estimates the state with respect to a local frame that is colocated with the odometry keyframe. As a result, the odometry update provides nearly direct measurements of the relative vehicle pose, making those states observable. Recent publications have rigorously documented the theoretical advantages of such an observable parameterization, including improved consistency, accuracy, and system robustness, and have demonstrated the effectiveness of such an approach during prolonged multirotor flight tests. This article complements this prior work by providing a complete, self-contained, tutorial derivation of the relative MEKF, which has been thoroughly motivated but only briefly described to date. This article presents several improvements and extensions to the filter while clearly defining all quaternion conventions and properties used, including several new useful properties relating to error quaternions and their Euler-angle decomposition. Finally, this article derives the filter both for traditional dynamics defined with respect to an inertial frame, and for robocentric dynamics defined with respect to the vehicle’s body frame, and provides insights into the subtle differences that arise between the two formulations.

Relative Navigation: A Keyframe-Based Approach for Observable GPS-Degraded Navigation

“…Reference [20] includes the details necessary to implement a relative state estimator for a MAV, including the vehicle dynamics and measurement models. The estimator implementations of each approach for the results in this article are all based on the error-state, multiplicative extended Kalman filter described in [20]. The changes needed to adapt this filter to each of the different estimation approaches were minimal, requiring modifications to less than ten lines of code for each approach.…”

“…Because conventional multirotor dynamics assume an inertial reference frame, the robocentric displacement vectorx ∆ in Figure 2c cannot be propagated directly. Instead, following [35] kRC was implemented using vehicle dynamics expressed with respect to the body, also described in [20]. Because these position dynamics do not depend on the current attitude, the EKF has no mechanism to properly increase position uncertainty due to heading uncertainty.…”

“…The following sections give highlevel descriptions of the relative front end and global back end. For further details on the RN approach and its implementation, the reader is referred to [20][21][22].…”

Measurement model and observability analysis for optical flow-aided inertial navigation

“…And it is exceptive for the quaternion error, which was defined by the 3 × 1 angleerror vector δθ, as δq ≜ ½ 1 2 δθ T 1 T . Then, the linearized first-order approximation of continuous-time error-state kinematics 14,20,24…”

“…Here, Γ T 0 means the incremental rotation matrix if rotating an arbitrary coordinate frame with a rotational rate of −ω for timespan Δt according to the Rodrigues rotation formula; therefore, the discrete linearized error dynamic transition matrix Φ k as a function of above series 24 (for readabilitŷ Γ T n ¼ Γ T n ½ω m ðkÞ −b ω ðkÞ) can be written as in matrix E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 4 ; 6 3 ; 4 3 8…”