#LyX 2.0 created this file. For more info see http://www.lyx.org/ \lyxformat 413 \begin_document \begin_header \textclass article \use_default_options true \maintain_unincluded_children false \language english \language_package default \inputencoding auto \fontencoding global \font_roman default \font_sans default \font_typewriter default \font_default_family default \use_non_tex_fonts false \font_sc false \font_osf false \font_sf_scale 100 \font_tt_scale 100 \graphics default \default_output_format default \output_sync 0 \bibtex_command default \index_command default \paperfontsize 11 \spacing single \use_hyperref false \papersize default \use_geometry true \use_amsmath 1 \use_esint 1 \use_mhchem 1 \use_mathdots 1 \cite_engine basic \use_bibtopic false \use_indices false \paperorientation portrait \suppress_date false \use_refstyle 1 \index Index \shortcut idx \color #008000 \end_index \leftmargin 3cm \topmargin 3cm \rightmargin 3cm \bottommargin 3cm \secnumdepth 3 \tocdepth 3 \paragraph_separation indent \paragraph_indentation default \quotes_language english \papercolumns 1 \papersides 1 \paperpagestyle default \tracking_changes false \output_changes false \html_math_output 0 \html_css_as_file 0 \html_be_strict false \end_header \begin_body \begin_layout Title The new IMU Factor \end_layout \begin_layout Author Frank Dellaert \end_layout \begin_layout Standard Let us assume a setup where frames with image and/or laser measurements are processed at some fairly low rate, e.g., 10 Hz. We define the state of the vehicle at those times as attitude, position, and velocity. These three quantities are jointly referred to as a NavState \begin_inset Formula $X\doteq(R_{b}^{n},p^{n},v^{n})$ \end_inset , where the superscript \begin_inset Formula $n$ \end_inset denotes the \emph on navigation frame \emph default , and \begin_inset Formula $b$ \end_inset the \emph on body frame \emph default . For simplicity, we drop these indices below where clear from context. \end_layout \begin_layout Standard Let us consider a simplified situation where we have a perfect gyroscope and accelerometer, i.e., assuming zero noise and bias terms, where the angular velocity \begin_inset Formula $\omega$ \end_inset and acceleration \begin_inset Formula $a$ \end_inset are measured in the body frame. Then we can model the state of the vehicle as governed by the following kinematic equations, \begin_inset Formula \begin{eqnarray} \dot{R} & = & R\hat{\omega}\label{eq:diffeq}\\ \dot{p} & = & v\label{eq:diffeq2}\\ \dot{v} & = & g+Ra\label{eq:diffeq3} \end{eqnarray} \end_inset Note that gravity is not measured by an accelerometer and needs to be added separately. \end_layout \begin_layout Standard We only measure \begin_inset Formula $\omega$ \end_inset and \begin_inset Formula $a$ \end_inset at discrete instants of time, and hence we need to make choices about how to integrate the equations above numerically. For a vehicle such as a quadrotor UAV, it is not a bad assumption to model \begin_inset Formula $\omega$ \end_inset and \begin_inset Formula $a$ \end_inset as piecewise constant in the body frame, as the actuation is fixed to the body. \end_layout \begin_layout Standard \begin_inset Note Note status open \begin_layout Plain Layout This motivates splitting up the integration into two parts: one that depends on knowing the exact rotation at the beginning of the interval, and another that does not: \begin_inset Formula \begin{eqnarray*} R(t) & = & R_{0}\int_{0}^{t}R_{0}^{T}R(\tau)\hat{\omega}(\tau)d\tau\\ \dot{p} & = & R_{0}\int_{0}^{t}R_{0}^{T}v(\tau)d\tau\\ \dot{v} & = & \int_{0}^{t}gd\tau+R_{0}\int_{0}^{t}R_{0}^{T}R(\tau)a(\tau)d\tau \end{eqnarray*} \end_inset \end_layout \end_inset \end_layout \begin_layout Standard It is well known that constant angular and linear velocity, expressed in the body frame, integrate to a spiral trajectory. This is captured exactly by the exponential map of \begin_inset Formula $SE(3)$ \end_inset : \begin_inset Formula \[ \left[\begin{array}{cc} R & p\\ 0 & 1 \end{array}\right]=\lim_{n\rightarrow\infty}\left(I+\left[\begin{array}{cc} \hat{\omega} & v^{b}\\ 0 & 0 \end{array}\right]\frac{\Delta t}{n}\right)^{n}\doteq\exp\left[\begin{array}{cc} \hat{\omega} & v^{b}\\ 0 & 0 \end{array}\right]\Delta t \] \end_inset As can be seen from the definition, the exponential map directly derives from dividing up a finite interval \begin_inset Formula $\Delta t$ \end_inset into an infinite number of infinitesimally small intervals \begin_inset Formula $\Delta t/n$ \end_inset . As a consequence, integrating the kinematics forward in \begin_inset Formula $SE(3)$ \end_inset translates simply to \emph on multiplication \emph default with \begin_inset Formula $\Delta t$ \end_inset in the 6-dimensional tangent space. \end_layout \begin_layout Standard What is needed to achieve the same understanding for NavStates, integrated forward under constant angular velocity and linear acceleration? For \begin_inset Formula $SE(3)$ \end_inset , the exponential map arose naturally when we embedded the 6-dimensional manifold in \begin_inset Formula $GL(4)$ \end_inset , the space of all non-singular \begin_inset Formula $4\times4$ \end_inset matrices. We can try the same trick with NavStates, e.g., embedding them in \begin_inset Formula $GL(7)$ \end_inset using the following representation: \begin_inset Formula \[ X=\left[\begin{array}{ccc} R & & p\\ & R & v\\ & & 1 \end{array}\right] \] \end_inset However, the induced group operation does not really do what we want, nor are NavStates even expected to behave as a group. \end_layout \begin_layout Standard \begin_inset Note Note status open \begin_layout Plain Layout The group operation inherited from \begin_inset Formula $GL(7)$ \end_inset yields the following result, \begin_inset Formula \[ \left[\begin{array}{ccc} R_{1} & & p_{1}\\ & R_{1} & v_{1}\\ & & 1 \end{array}\right]\left[\begin{array}{ccc} R_{2} & & p_{2}\\ & R_{2} & v_{2}\\ & & 1 \end{array}\right]=\left[\begin{array}{ccc} R_{1}R_{2} & & p_{1}+R_{1}p_{2}\\ & R_{1}R_{2} & v_{1}+R_{1}v_{2}\\ & & 1 \end{array}\right] \] \end_inset i.e., \begin_inset Formula $R_{2}$ \end_inset , \begin_inset Formula $p_{2}$ \end_inset , and \begin_inset Formula $v_{2}$ \end_inset are to interpreted as quantities in the body frame. How can we achieve a constant angular velocity/constant acceleration operation with this representation? For an infinitesimal interval \begin_inset Formula $\delta$ \end_inset , we expect the result to be \begin_inset Formula \[ \left[\begin{array}{ccc} R+R\hat{\omega}\delta & & p+v\delta\\ & R+R\hat{\omega}\delta & v+Ra\delta\\ & & 1 \end{array}\right] \] \end_inset This can NOT be achieved by \begin_inset Formula \[ \left[\begin{array}{ccc} R & & p\\ & R & v\\ & & 1 \end{array}\right]\left[\begin{array}{ccc} I+\hat{\omega}\delta & \delta & 0\\ & I+\hat{\omega}\delta & a\delta\\ & & 1 \end{array}\right] \] \end_inset because it is not closed. Hence, the exponential map as defined below does not really do it for us \begin_inset Formula \[ \left[\begin{array}{ccc} R & & p\\ & R & v\\ & & 1 \end{array}\right]=\lim_{n\rightarrow\infty}\left(I+\left[\begin{array}{ccc} \hat{\omega} & & v^{b}\\ & \hat{\omega} & a\\ & & 1 \end{array}\right]\frac{\Delta t}{n}\right)^{n}=\left[\begin{array}{ccc} R+R\hat{\omega}\delta & & p+v\delta\\ & R+R\hat{\omega}\delta & v+Ra\delta\\ & & 1 \end{array}\right] \] \end_inset \end_layout \end_inset \end_layout \begin_layout Standard We can still, however, treat the NavState as living in a 9-dimensional manifold \begin_inset Formula $M$ \end_inset , defined by the orthonormality constraints on \begin_inset Formula $R$ \end_inset . In mechanics, a natural manifold associated with \begin_inset Formula $SE(3)$ \end_inset is its \emph on tangent bundle \emph default , which is 12-dimensional and consists of pairs \begin_inset Formula $((R,p),(\omega,v))$ \end_inset , and is useful for integrating the Euler-Lagrange equations of motion. However, in an inertial navigation context, we measure \begin_inset Formula $\omega$ \end_inset and \begin_inset Formula $a$ \end_inset , and we can make do with the 9-dimensional manifold \begin_inset Formula $M$ \end_inset consisting of the triples \begin_inset Formula $(R,p,v)$ \end_inset . \end_layout \begin_layout Standard Under constant angular velocity and linear acceleration, in the body frame, we obtain a family of trajectories \begin_inset Formula $\gamma(t):R\rightarrow M$ \end_inset by integrating \begin_inset Formula \begin{eqnarray*} R(t) & = & \int_{0}^{t}R(\tau)\hat{\omega}d\tau\\ p(t) & = & \int_{0}^{t}v(\tau)d\tau\\ v(t) & = & \int_{0}^{t}\left\{ g+R(\tau)a\right\} d\tau \end{eqnarray*} \end_inset with \begin_inset Formula $\gamma(0)=(R_{0},p_{0},v_{0})$ \end_inset . In analogy to \begin_inset Formula $SE(3)$ \end_inset we know that (Murray94book, page 42): \begin_inset Formula \begin{eqnarray*} R(t) & = & R_{0}\exp\hat{\omega}t\\ v(t) & = & v_{0}+gt+R_{0}\left\{ (I-\exp\hat{\omega}t)\left(\omega\times a\right)+\omega\omega^{T}at\right\} \end{eqnarray*} \end_inset Plugging that into position yields \begin_inset Formula \begin{eqnarray*} p(t) & = & p_{0}+v_{0}t+g\frac{t^{2}}{2}+R_{0}\int_{0}^{t}\left\{ (I-\exp\hat{\omega}\tau)\left(\omega\times a\right)+\omega\omega^{T}a\tau\right\} d\tau \end{eqnarray*} \end_inset where the last term integrates the velocity spiral induced by constant accelerat ion in the rotating body frame. \end_layout \begin_layout Standard It is worth asking what all this complexity buys us, however. Even for a quadrotor, forces induced by wind effects and drag are arguably better approximated over short intervals as constant in the navigation frame. And gravity is clearly constant in the navigation frame, but \emph on not \emph default in the body frame. \begin_inset Note Note status collapsed \begin_layout Plain Layout More so, if we do not know \begin_inset Formula $R$ \end_inset perfectly, integrating gravity in the body frame could lead to significant errors, as gravity typically dominates other accelerations in the system. \end_layout \end_inset \end_layout \begin_layout Standard Let us examine what we obtain using a constant angular velocity in the body, but with a zero-order hold on acceleration in the navigation frame instead. While \begin_inset Formula $a$ \end_inset is still measured in the body frame, we rotate it once by \begin_inset Formula $R_{0}$ \end_inset at \begin_inset Formula $t=0$ \end_inset , and we obtain a much simplified picture: \begin_inset Formula \begin{eqnarray*} R(t) & = & R_{0}\exp\hat{\omega}t\\ v(t) & = & v_{0}+\left(g+R_{0}a\right)t \end{eqnarray*} \end_inset Plugging this into position now yields a much simpler equation, as well: \begin_inset Formula \begin{eqnarray*} p(t) & = & p_{0}+v_{0}t+\left(g+R_{0}a\right)\frac{t^{2}}{2} \end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status collapsed \begin_layout Plain Layout In the context of the IMU factor it is useful to describe these trajectories in a manner that does not depend on the initial NavState \begin_inset Formula $(R_{0},p_{0},v_{0})$ \end_inset . Here is an attempt: \end_layout \begin_layout Plain Layout \begin_inset Formula \begin{eqnarray*} R(t) & = & \int_{0}^{t}R(\tau)\hat{\omega}d\tau\\ p(t) & = & \int_{0}^{t}v(\tau)d\tau\\ v(t) & = & \int_{0}^{t}R(\tau)ad\tau \end{eqnarray*} \end_inset \end_layout \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status collapsed \begin_layout Plain Layout We now choose to define the retraction from the tangent space at \begin_inset Formula $X$ \end_inset back to the manifold as \begin_inset Formula \[ X\oplus dX=(R,p,v)\oplus(d_{R},d_{p},d_{v})\doteq(R\exp d_{R},p+Rd_{p},v+Rd_{v}) \] \end_inset A crucial detail is that the incremental position \begin_inset Formula $d_{p}$ \end_inset and velocity \begin_inset Formula $d_{v}$ \end_inset are given in the NavState frame, rather than in the global frame, and hence are rotated by \begin_inset Formula $R$ \end_inset before applying. This is in analogy to \begin_inset Formula $SE(3)$ \end_inset , treating velocity here in the same way as position in \begin_inset Formula $SE(3)$ \end_inset . \end_layout \end_inset \end_layout \begin_layout Standard The goal of the IMU factor is to integrate IMU measurements between two successive frames and create a binary factor between two NavStates. Integrating successive gyro and accelerometer readings using the above equations over each constant interval yields \begin_inset Formula \begin{eqnarray} R_{k+1} & = & R_{k}\exp\hat{\omega}_{k}\Delta t_{k}\label{eq:iter_Rk}\\ p_{k+1} & = & p_{k}+v_{k}\Delta t_{k}+\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}=p_{k}+\frac{v_{k}+v_{k+1}}{2}\Delta t_{k}\nonumber \\ v_{k+1} & = & v_{k}+(g+R_{k}a_{k})\Delta t_{k}\nonumber \end{eqnarray} \end_inset Starting \begin_inset Formula $X_{i}=(R_{i},p_{i},v_{i})$ \end_inset , we then obtain an expression for \begin_inset Formula $X_{j}$ \end_inset , \begin_inset Formula \begin{eqnarray*} R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\ p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\ v_{j} & = & v_{i}+\sum_{k}(g+R_{k}a_{k})\Delta t_{k} \end{eqnarray*} \end_inset where \begin_inset Formula $R_{k}$ \end_inset has to be updated as in \begin_inset CommandInset ref LatexCommand formatted reference "eq:iter_Rk" \end_inset . Note that \begin_inset Formula \[ v_{k}=v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l} \] \end_inset and hence \begin_inset Formula \[ p_{j}=p_{i}+\sum_{k}\left(v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2} \] \end_inset \end_layout \begin_layout Standard A crucial problem here is that we depend on a good estimate of \begin_inset Formula $R_{k}$ \end_inset at all times, which includes the potentially wrong estimate for the initial attitude \begin_inset Formula $R_{i}$ \end_inset . \end_layout \begin_layout Standard The idea behind the preintegrated IMU factor is two-fold: (a) treat gravity separately, in the navigation frame, and (b) instead of integrating in absolute coordinates, we want the IMU integration to happen in the frame \begin_inset Formula $(R_{i},p_{i},v_{i})$ \end_inset . The first idea is easily incorporated by separating out all gravity-related components: \begin_inset Formula \begin{eqnarray*} p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\ v_{j} & = & v_{i}+g\sum_{k}\Delta t_{k}+\sum_{k}R_{k}a_{k}\Delta t_{k} \end{eqnarray*} \end_inset \end_layout \begin_layout Standard But we need to define what that means: clearly, \begin_inset Formula $SE(3)$ \end_inset , with its group structure, has a well-defined concept of working \begin_inset Quotes eld \end_inset in the frame \begin_inset Quotes erd \end_inset . But in this case we have a manifold, not a group. To deal with this, we perform the integration in the tangent space, and hence we need to define a mapping from tangent space to manifold and vice versa. A possible definition of retract, compatible with the semi-direct group structure of \begin_inset Formula $SE(3)$ \end_inset and treating velocities the same way as positions, is given by \begin_inset Formula \[ X\oplus dX=(R,p,v)\oplus(\omega t,\Delta p,\Delta v)=(R\exp\hat{\omega}t,p+R\Delta p,v+R\Delta v) \] \end_inset \begin_inset Formula \begin{eqnarray*} R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\ p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\frac{1}{2}g\sum_{k}\Delta t_{k}^{2}+\frac{1}{2}\sum_{k}R_{k}a_{k}\Delta t^{2}\\ v_{j} & = & v_{i}+g\Delta t_{ij}+\sum_{k}R_{k}a_{k}\Delta t_{k} \end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status open \begin_layout Plain Layout The binary factor is set up as a prediction: \begin_inset Formula \[ X_{j}\approx X_{i}\oplus dX_{ij} \] \end_inset where \begin_inset Formula $dX_{ij}$ \end_inset is a tangent vector in the tangent space \begin_inset Formula $T_{i}$ \end_inset to the manifold at \begin_inset Formula $X_{i}$ \end_inset . \end_layout \begin_layout Plain Layout Integrating gyro and accelerometer readings, following Forster05rss, and assuming zero bias for now, is done by: \begin_inset Formula \begin{eqnarray*} R_{j} & = & R_{i}\prod_{k}\exp\omega_{k}\Delta t\\ p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t+\frac{1}{2}g\Delta t_{ij}^{2}+\frac{1}{2}\sum_{k}R_{k}a_{k}\Delta t^{2}\\ v_{j} & = & v_{i}+g\Delta t_{ij}+\sum_{k}R_{k}a_{k}\Delta t \end{eqnarray*} \end_inset We would, however, like to separate out the constant velocity and gravity components from the IMU-induced terms: \begin_inset Formula \begin{eqnarray*} R_{j} & = & R_{i}\prod_{k}\exp\omega_{k}\Delta t\\ p_{j} & = & \left\{ p_{i}+v_{i}\Delta t_{ij}+\frac{1}{2}g\Delta t_{ij}^{2}\right\} +\sum_{k}\left(v_{k}-v_{i}\right)\Delta t+\frac{1}{2}\sum_{k}R_{k}a_{k}\Delta t^{2}\\ v_{j} & = & \left\{ v_{i}+g\Delta t_{ij}\right\} +\sum_{k}R_{k}a_{k}\Delta t \end{eqnarray*} \end_inset \end_layout \begin_layout Plain Layout Let us look at a single interval of the incremental terms: \begin_inset Formula \begin{eqnarray*} R_{j} & = & R_{i}\exp\omega\Delta t\\ p_{j} & = & p_{i}+v_{i}\Delta t+\frac{1}{2}g\Delta t^{2}+\frac{1}{2}R_{i}a\Delta t^{2}\\ v_{j} & = & v_{i}+g\Delta t+R_{i}a_{k}\Delta t \end{eqnarray*} \end_inset Comparing that with the definition of retract, we have \begin_inset Formula \[ R_{j}=R_{i}\oplus\left(\omega,R_{i}^{T}v_{i}+\frac{1}{2}R_{i}^{T}g\Delta t+\frac{1}{2}a\Delta t,R_{i}^{T}g+a_{k}\right)\Delta t \] \end_inset \end_layout \end_inset \end_layout \end_body \end_document