updated ImuFactor doc with details about CombinedImuFactor
Varun Agrawal
parent
a2bf0c4da4
commit
aa4a163480
|
|
@ -108,6 +108,222 @@ filename "macros.lyx"
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
|
||||
{\mathfrak{\mathbb{R}^{9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times3}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times6}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
|
||||
{\mathfrak{\mathbb{R}^{9\times9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU factor has 2 variants:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
ImuFactor is a 5-way factor between the previous pose and velocity, the
|
||||
current pose and velocity, and the current IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
ImuFactor2 is a 3-way factor between the previous NavState, the current
|
||||
NavState and the current IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Both variants take a PreintegratedMeasurements object which encodes all
|
||||
the IMU measurements between the previous timestep and the current timestep.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
There are also 2 variants of this class:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Manifold Preintegration: This version keeps track of the incremental NavState
|
||||
|
||||
\begin_inset Formula $\Delta X_{ij}$
|
||||
\end_inset
|
||||
|
||||
with respect to the previous NavState, on the NavState manifold itself.
|
||||
It also keeps track of the
|
||||
\begin_inset Formula $\Rninesix$
|
||||
\end_inset
|
||||
|
||||
Jacobian of
|
||||
\begin_inset Formula $\Delta X_{ij}$
|
||||
\end_inset
|
||||
|
||||
w.r.t.
|
||||
the bias.
|
||||
This corresponds to Forster et.
|
||||
al.
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Forster15rss"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Tangent Preintegration: This version keeps track of the incremental NavState
|
||||
in the NavState tangent space instead.
|
||||
This is a
|
||||
\begin_inset Formula $\Rnine$
|
||||
\end_inset
|
||||
|
||||
vector
|
||||
\emph on
|
||||
preintegrated_
|
||||
\emph default
|
||||
.
|
||||
It also keeps track of the
|
||||
\begin_inset Formula $\Rninesix$
|
||||
\end_inset
|
||||
|
||||
jacobian of the
|
||||
\emph on
|
||||
preintegrated_
|
||||
\emph default
|
||||
w.r.t.
|
||||
the bias.
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The main function of a factor is to calculate an error.
|
||||
The easiest case to look at is the NavState variant in ImuFactor2, which
|
||||
is given as:
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
\Delta X_{ij}=X_{j}-\hat{X_{ij}}\label{eq:imu-factor-error}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Combined IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU factor above requires that bias drift over time be modeled as a
|
||||
separate stochastic process (using a BetweenFactor for example), a crucial
|
||||
aspect given that the preintegrated measurements depend on these bias values
|
||||
and are thus correlated.
|
||||
For this reason, we provide another type of IMU factor which we term the
|
||||
Combined IMU Factor.
|
||||
This factor similarly has 2 variants:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
CombinedImuFactor is a 6-way factor between the previous pose, velocity
|
||||
and IMU bias and the current pose, velocity and IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
|
||||
bias and the current NavState and IMU bias.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Covariance Matrices
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
For IMU preintegration, it is important to propagate the uncertainty accurately
|
||||
as well.
|
||||
As such, we detail the various covariance matrices used in the preintegration
|
||||
step.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Gyroscope Covariance
|
||||
\begin_inset Formula $Q_{\omega}$
|
||||
\end_inset
|
||||
|
||||
: Measurement uncertainty of the gyroscope.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Accelerometer Covariance
|
||||
\begin_inset Formula $Q_{acc}$
|
||||
\end_inset
|
||||
|
||||
: Measurement uncertainty of the accelerometer.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Accelerometer Bias Covariance
|
||||
\begin_inset Formula $Q_{\Delta b^{acc}}$
|
||||
\end_inset
|
||||
|
||||
: The covariance associated with the accelerometer bias random walk.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Gyroscope Bias Covariance
|
||||
\begin_inset Formula $Q_{\Delta b^{\omega}}$
|
||||
\end_inset
|
||||
|
||||
: The covariance associated with the gyroscope bias random walk.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Integration Covariance
|
||||
\begin_inset Formula $Q_{int}$
|
||||
\end_inset
|
||||
|
||||
: This is the uncertainty due to modeling errors in the integration from
|
||||
acceleration to velocity and position.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
Initial Bias Estimate Covariance
|
||||
\begin_inset Formula $Q_{init}$
|
||||
\end_inset
|
||||
|
||||
: This is the uncertainty associated with the estimation of the bias (since
|
||||
we jointly estimate the bias as well).
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Navigation States
|
||||
\end_layout
|
||||
|
|
@ -725,15 +941,6 @@ In other words, the vector field
|
|||
Retractions
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset FormulaMacro
|
||||
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
|
||||
{\mathfrak{\mathbb{R}^{9}}}
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Note that the use of the exponential map in local coordinate mappings is
|
||||
not obligatory, even in the context of Lie groups.
|
||||
|
|
@ -1018,7 +1225,15 @@ In the IMU factor, we need to predict the NavState
|
|||
|
||||
needs to be known in order to compensate properly for the initial velocity
|
||||
and rotated gravity vector.
|
||||
Hence, the idea of Lupton was to split up
|
||||
Hence, the idea of Lupton
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Lupton12tro"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
was to split up
|
||||
\begin_inset Formula $v(t)$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1075,8 +1290,11 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
|
|||
|
||||
\end_inset
|
||||
|
||||
The recipe for the IMU factor is then, in summary.
|
||||
Solve the ordinary differential equations
|
||||
The recipe for the IMU factor is then, in summary:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Solve the ordinary differential equations
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
\dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\
|
||||
|
|
@ -1095,7 +1313,10 @@ starting from zero, up to time
|
|||
\end_inset
|
||||
|
||||
at all times.
|
||||
Form the local coordinate vector as
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Form the local coordinate vector as
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right]
|
||||
|
|
@ -1103,6 +1324,10 @@ starting from zero, up to time
|
|||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Enumerate
|
||||
Predict the NavState
|
||||
\begin_inset Formula $X_{j}$
|
||||
\end_inset
|
||||
|
|
@ -1197,7 +1422,50 @@ where we defined the rotation matrix
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation
|
||||
Noise Modeling
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Given the above solutions to the differential equations, we add noise modeling
|
||||
to account for the various sources of error in the system
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray}
|
||||
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega} -b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
|
||||
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
|
||||
v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
|
||||
b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
|
||||
b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber
|
||||
\end{eqnarray}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
which we can write compactly as,
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray}
|
||||
\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
|
||||
p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
|
||||
v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
|
||||
b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
|
||||
b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber
|
||||
\end{eqnarray}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation in IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
|
|
@ -1257,7 +1525,7 @@ Then the noise on
|
|||
propagates as
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
|
||||
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1313,7 +1581,7 @@ We start with the noise propagation on
|
|||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
|
||||
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1325,7 +1593,7 @@ It can be shown that for small
|
|||
we have
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
|
||||
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1375,9 +1643,9 @@ Putting all this together, we finally obtain
|
|||
\begin_inset Formula
|
||||
\[
|
||||
A_{k}\approx\left[\begin{array}{ccc}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{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}
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
|
|
@ -1403,26 +1671,12 @@ H(\theta_{k})^{-1}\Delta_{t}\\
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Combined IMU Factor
|
||||
Noise Propagation in Combined IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We can similarly account for bias drift over time, as is commonly seen in
|
||||
commercial grade IMUs.
|
||||
This is accomplished via the
|
||||
\emph on
|
||||
CombinedImuFactor
|
||||
\emph default
|
||||
which is a 6-way factor between the previous
|
||||
\emph on
|
||||
pose/velocity/bias
|
||||
\emph default
|
||||
and the
|
||||
\emph on
|
||||
pose/velocity/bias
|
||||
\emph default
|
||||
at the next timestep.
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
|
|
@ -1430,14 +1684,18 @@ We expand the state vector as
|
|||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
|
||||
\end_inset
|
||||
|
||||
to include the bias terms.
|
||||
to include the bias terms and define the augmented noise vector
|
||||
\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
This gives the noise propagation equation as
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}\Sigma_{k}G_{k}\label{eq:prop-combined}
|
||||
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
|
@ -1467,10 +1725,19 @@ where
|
|||
\end_inset
|
||||
|
||||
is the
|
||||
\begin_inset Formula $15\times15$
|
||||
\begin_inset Formula $15\times21$
|
||||
\end_inset
|
||||
|
||||
matrix for first order uncertainty propagation.
|
||||
|
||||
\begin_inset Formula $Q_{k}$
|
||||
\end_inset
|
||||
|
||||
defines the uncertainty of
|
||||
\begin_inset Formula $\eta$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
The top-left
|
||||
\begin_inset Formula $9\times9$
|
||||
\end_inset
|
||||
|
|
@ -1509,22 +1776,213 @@ derivation as matrices
|
|||
\begin_inset Formula
|
||||
\[
|
||||
F_{k}\approx\left[\begin{array}{ccccc}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & & & & H(\theta_{k})^{-1}\Delta_{t}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} & R_{k}\Delta_{t}\\
|
||||
& & & I_{3\times3}\\
|
||||
& & & & I_{3\times3}
|
||||
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
|
||||
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
|
||||
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
|
||||
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Similarly for
|
||||
\begin_inset Formula $Q_{k},$
|
||||
\end_inset
|
||||
|
||||
we get
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
Q_{k}=\left[\begin{array}{ccccccc}
|
||||
\Sigma^{\omega}\\
|
||||
& \Sigma^{a}\\
|
||||
& & \Sigma^{b^{a}}\\
|
||||
& & & \Sigma^{b^{\omega}}\\
|
||||
& & & & \Sigma^{int}\\
|
||||
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
||||
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
and for
|
||||
\begin_inset Formula $G_{k}$
|
||||
\end_inset
|
||||
|
||||
we get
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
|
||||
\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
|
||||
\end{array}\right]=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
||||
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We can perform the block-wise computation of
|
||||
\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
|
||||
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
|
||||
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
|
||||
\end{array}\right]\left[\begin{array}{ccccccc}
|
||||
\Sigma^{\omega}\\
|
||||
& \Sigma^{a}\\
|
||||
& & \Sigma^{b^{a}}\\
|
||||
& & & \Sigma^{b^{\omega}}\\
|
||||
& & & & \Sigma^{int}\\
|
||||
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
|
||||
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
|
||||
\end{array}\right]G_{k}^{T}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
||||
\end{array}\right]G_{k}^{T}
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
|
||||
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
|
||||
\end{array}\right]\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}} & \deriv v{\epsilon^{a}} & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0\\
|
||||
0 & 0 & 0 & 0 & I_{3\times3}\\
|
||||
0 & \deriv p{\epsilon^{int}} & 0 & 0 & 0\\
|
||||
0 & \deriv p{\eta_{init}^{b^{a}}} & \deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
=\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}\\
|
||||
& +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
||||
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
which we can break into 3 matrices for clarity, representing the main diagonal
|
||||
and off-diagonal elements
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\begin{multline*}
|
||||
=\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}} & 0 & 0\\
|
||||
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
|
||||
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
|
||||
\end{array}\right]+\\
|
||||
\left[\begin{array}{ccccc}
|
||||
\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0
|
||||
\end{array}\right]+\\
|
||||
\left[\begin{array}{ccccc}
|
||||
0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset CommandInset bibtex
|
||||
LatexCommand bibtex
|
||||
btprint "btPrintCited"
|
||||
bibfiles "refs"
|
||||
options "plain"
|
||||
|
||||
|
|
|
|||
Binary file not shown.
68
doc/refs.bib
68
doc/refs.bib
|
|
@ -1,26 +1,52 @@
|
|||
%% This BibTeX bibliography file was created using BibDesk.
|
||||
%% https://bibdesk.sourceforge.io/
|
||||
|
||||
%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400
|
||||
|
||||
|
||||
%% Saved with string encoding Unicode (UTF-8)
|
||||
|
||||
|
||||
|
||||
@article{Lupton12tro,
|
||||
author = {Lupton, Todd and Sukkarieh, Salah},
|
||||
date-added = {2021-09-27 17:38:56 -0400},
|
||||
date-modified = {2021-09-27 17:39:09 -0400},
|
||||
doi = {10.1109/TRO.2011.2170332},
|
||||
journal = {IEEE Transactions on Robotics},
|
||||
number = {1},
|
||||
pages = {61-76},
|
||||
title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
|
||||
volume = {28},
|
||||
year = {2012},
|
||||
Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
|
||||
|
||||
@inproceedings{Forster15rss,
|
||||
author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
|
||||
booktitle = {Robotics: Science and Systems},
|
||||
date-added = {2021-09-26 20:44:41 -0400},
|
||||
date-modified = {2021-09-26 20:45:03 -0400},
|
||||
title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
|
||||
year = {2015}}
|
||||
|
||||
@article{Iserles00an,
|
||||
title = {Lie-group methods},
|
||||
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
|
||||
N{\o}rsett, Syvert P and Zanna, Antonella},
|
||||
journal = {Acta Numerica 2000},
|
||||
volume = {9},
|
||||
pages = {215--365},
|
||||
year = {2000},
|
||||
publisher = {Cambridge Univ Press}
|
||||
}
|
||||
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
|
||||
journal = {Acta Numerica 2000},
|
||||
pages = {215--365},
|
||||
publisher = {Cambridge Univ Press},
|
||||
title = {Lie-group methods},
|
||||
volume = {9},
|
||||
year = {2000}}
|
||||
|
||||
@book{Murray94book,
|
||||
title = {A mathematical introduction to robotic manipulation},
|
||||
author = {Murray, Richard M and Li, Zexiang and Sastry, S
|
||||
Shankar and Sastry, S Shankara},
|
||||
year = {1994},
|
||||
publisher = {CRC press}
|
||||
}
|
||||
author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
|
||||
publisher = {CRC press},
|
||||
title = {A mathematical introduction to robotic manipulation},
|
||||
year = {1994}}
|
||||
|
||||
@book{Spivak65book,
|
||||
title = {Calculus on manifolds},
|
||||
author = {Spivak, Michael},
|
||||
volume = {1},
|
||||
year = {1965},
|
||||
publisher = {WA Benjamin New York}
|
||||
}
|
||||
author = {Spivak, Michael},
|
||||
publisher = {WA Benjamin New York},
|
||||
title = {Calculus on manifolds},
|
||||
volume = {1},
|
||||
year = {1965}}
|
||||
|
|
|
|||
Loading…
Reference in New Issue