830 lines
18 KiB
Plaintext
830 lines
18 KiB
Plaintext
#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
|