Noise propagation
Frank Dellaert
parent
43520265aa
commit
be658119b9
|
|
@ -1196,7 +1196,7 @@ Even when we assume uncorrelated noise on
|
|||
\begin_inset Formula $\theta_{k}$
|
||||
\end_inset
|
||||
|
||||
and
|
||||
and
|
||||
\begin_inset Formula $v_{k}$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1213,7 +1213,7 @@ reference "eq:euler_theta"
|
|||
|
||||
\end_inset
|
||||
|
||||
|
||||
-
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand ref
|
||||
reference "eq:euler_v"
|
||||
|
|
@ -1227,7 +1227,7 @@ reference "eq:euler_v"
|
|||
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\zeta_{k+1}=f\left(\zeta_{k},\omega_{k}^{b},a_{k}^{b}\right)
|
||||
\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1283,6 +1283,15 @@ where
|
|||
.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We start with the noise propagation on
|
||||
\begin_inset Formula $\theta$
|
||||
\end_inset
|
||||
|
||||
, which is independent of the other quantities.
|
||||
Taking the derivative, we have
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
|
|
@ -1314,7 +1323,7 @@ For the derivatives of
|
|||
we need the derivative
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})
|
||||
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1322,7 +1331,7 @@ For the derivatives of
|
|||
where we used
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{\left(Ra\right)}R\approx-R\Skew a
|
||||
\deriv{\left(Ra\right)}R\approx R\Skew{-a}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1349,8 +1358,8 @@ Putting all this together, we finally obtain
|
|||
\[
|
||||
A_{k}\approx\left[\begin{array}{ccc}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
|
||||
-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
|
||||
-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
|
|
@ -1373,314 +1382,6 @@ H(\theta_{k})^{-1}\Delta_{t}\\
|
|||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
A more accurate partial derivative of
|
||||
\begin_inset Formula $H(\theta_{k})^{-1}$
|
||||
\end_inset
|
||||
|
||||
can be used, as well.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Section
|
||||
Old Stuff:
|
||||
\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 collapsed
|
||||
|
||||
\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
|
||||
For a quadrotor, forces induced by wind effects and drag are arguably better
|
||||
approximated over short intervals as constant in the navigation frame.
|
||||
\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
|
||||
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
|
||||
[Is there not a 3/2 power here as in the RSS paper?]
|
||||
\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}\left(\sum_{l}g\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(v_{i}+\sum_{l}R_{l}a_{l}\Delta t_{l}\right)\Delta t_{k}+\sum_{k}g\frac{\left(\Delta t_{k}\right)^{2}}{2}+\sum_{k}R_{k}a_{k}\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
|
||||
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 Standard
|
||||
|
|
|
|||
Loading…
Reference in New Issue