Merge pull request #879 from borglab/fix/combined-imu-cov
Fix Preintegration Covariance of CombinedImuFactorrelease/4.3a0
Varun Agrawal
GitHub
commit
c767dfafb3
|
|
@ -82,7 +82,7 @@
|
|||
\begin_body
|
||||
|
||||
\begin_layout Title
|
||||
The new IMU Factor
|
||||
The New IMU Factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Author
|
||||
|
|
@ -108,6 +108,282 @@ 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.
|
||||
This is done exactly the same in both variants:
|
||||
\begin_inset Formula
|
||||
\begin{equation}
|
||||
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
||||
where the predicted NavState
|
||||
\begin_inset Formula $\widehat{X_{j}}$
|
||||
\end_inset
|
||||
|
||||
at time
|
||||
\begin_inset Formula $t_{j}$
|
||||
\end_inset
|
||||
|
||||
is a function of the NavState
|
||||
\begin_inset Formula $X_{i}$
|
||||
\end_inset
|
||||
|
||||
at time
|
||||
\begin_inset Formula $t_{i}$
|
||||
\end_inset
|
||||
|
||||
and the preintegrated measurements
|
||||
\begin_inset Formula $PIM$
|
||||
\end_inset
|
||||
|
||||
:
|
||||
\begin_inset Formula
|
||||
\[
|
||||
\widehat{X_{j}}=f(X_{i},PIM)
|
||||
\]
|
||||
|
||||
\end_inset
|
||||
|
||||
The noise model associated with this factor is assumed to be zero-mean Gaussian
|
||||
with a
|
||||
\begin_inset Formula $9\times9$
|
||||
\end_inset
|
||||
|
||||
covariance matrix
|
||||
\begin_inset Formula $\Sigma_{ij}$
|
||||
\end_inset
|
||||
|
||||
, which is defined in the tangent space
|
||||
\begin_inset Formula $T_{X_{j}}\mathcal{N}$
|
||||
\end_inset
|
||||
|
||||
of the NavState manifold at the NavState
|
||||
\begin_inset Formula $X_{j}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
This (discrete-time) covariance matrix is computed in the preintegrated
|
||||
measurement class, of which there are two variants as discussed above.
|
||||
\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 Standard
|
||||
Since the Combined IMU Factor has a larger state variable due to the inclusion
|
||||
of IMU biases, the noise model associated with this factor is assumed to
|
||||
be a zero mean Gaussian with a
|
||||
\begin_inset Formula $15\times15$
|
||||
\end_inset
|
||||
|
||||
covariance matrix
|
||||
\begin_inset Formula $\Sigma$
|
||||
\end_inset
|
||||
|
||||
, similarly defined on the tangent space of the NavState manifold.
|
||||
\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
|
||||
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
|
||||
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
|
||||
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
|
||||
|
|
@ -302,7 +578,15 @@ acceleration
|
|||
\end_inset
|
||||
|
||||
in the body frame.
|
||||
We know (from Murray84book) that the derivative of
|
||||
We know (from
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
key "Murray94book"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
) that the derivative of
|
||||
\begin_inset Formula $R$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -725,15 +1009,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 +1293,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 +1358,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 +1381,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 +1392,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,11 +1490,59 @@ where we defined the rotation matrix
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Noise Propagation
|
||||
Noise Modeling
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Even when we assume uncorrelated noise on
|
||||
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
|
||||
We wish to compute the ImuFactor covariance matrix
|
||||
\begin_inset Formula $\Sigma_{ij}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Even when we assume uncorrelated noise on
|
||||
\begin_inset Formula $\omega^{b}$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1219,11 +1560,12 @@ Even when we assume uncorrelated noise on
|
|||
\end_inset
|
||||
|
||||
appear in multiple places.
|
||||
To model the noise propagation, let us define
|
||||
To model the noise propagation, let us define the preintegrated navigation
|
||||
state
|
||||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
|
||||
\end_inset
|
||||
|
||||
and rewrite Eqns.
|
||||
, as a 9D vector on tangent space at and rewrite Eqns.
|
||||
(
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand ref
|
||||
|
|
@ -1257,7 +1599,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
|
||||
|
|
@ -1298,6 +1640,42 @@ where
|
|||
\begin_inset Formula $\omega^{b}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Note that
|
||||
\begin_inset Formula $\Sigma_{k},$
|
||||
\end_inset
|
||||
|
||||
|
||||
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
||||
\end_inset
|
||||
|
||||
, and
|
||||
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
||||
\end_inset
|
||||
|
||||
are discrete time covariances with
|
||||
\begin_inset Formula $\Sigma_{\eta}^{ad}$
|
||||
\end_inset
|
||||
|
||||
, and
|
||||
\begin_inset Formula $\Sigma_{\eta}^{gd}$
|
||||
\end_inset
|
||||
|
||||
divided by
|
||||
\begin_inset Formula $\Delta_{t}$
|
||||
\end_inset
|
||||
|
||||
.
|
||||
Please see the section on Covariance Discretization
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand vpageref
|
||||
reference "subsec:Covariance-Discretization"
|
||||
plural "false"
|
||||
caps "false"
|
||||
noprefix "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
.
|
||||
\end_layout
|
||||
|
||||
|
|
@ -1313,7 +1691,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 +1703,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 +1753,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,39 +1781,48 @@ 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
|
||||
We expand the state vector as
|
||||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},a_{k}^{b}, \omega_{k}^{b}]$
|
||||
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
|
||||
\end_inset
|
||||
|
||||
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
|
||||
|
||||
.
|
||||
For the jacobian
|
||||
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}Q_{k}G_{k}^{T}\label{eq:prop-combined}
|
||||
\end{equation}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
where
|
||||
\begin_inset Formula $F_{k}$
|
||||
\end_inset
|
||||
|
||||
of
|
||||
is the
|
||||
\begin_inset Formula $15\times15$
|
||||
\end_inset
|
||||
|
||||
derivative of
|
||||
\begin_inset Formula $f$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1443,13 +1830,30 @@ We expand the state vector as
|
|||
\begin_inset Formula $\zeta$
|
||||
\end_inset
|
||||
|
||||
, we get a
|
||||
\begin_inset Formula $15\times15$
|
||||
, and
|
||||
\begin_inset Formula $G_{k}$
|
||||
\end_inset
|
||||
|
||||
matrix.
|
||||
is the
|
||||
\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
|
||||
|
||||
of
|
||||
\begin_inset Formula $F_{k}$
|
||||
\end_inset
|
||||
|
||||
is the same as
|
||||
|
|
@ -1481,12 +1885,12 @@ derivation as matrices
|
|||
\begin_layout Standard
|
||||
\begin_inset Formula
|
||||
\[
|
||||
F_{k}=\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}
|
||||
F_{k}\approx\left[\begin{array}{ccccc}
|
||||
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]
|
||||
\]
|
||||
|
||||
|
|
@ -1495,9 +1899,274 @@ R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} & R_{k}\Delta_{t
|
|||
|
||||
\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}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
|
||||
0 & 0 & 0 & I_{3\times3} & 0\\
|
||||
0 & 0 & 0 & 0 & I_{3\times3}\\
|
||||
0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
|
||||
0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 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}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
|
||||
& +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 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}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 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}}}^{T} & 0 & 0 & 0 & 0\\
|
||||
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 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}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
|
||||
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0\\
|
||||
0 & 0 & 0 & 0 & 0
|
||||
\end{array}\right]
|
||||
\end{multline*}
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
Covariance Discretization
|
||||
\begin_inset CommandInset label
|
||||
LatexCommand label
|
||||
name "subsec:Covariance-Discretization"
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
So far, all the covariances are assumed to be continuous since the state
|
||||
and measurement models are considered to be continuous-time stochastic
|
||||
processes.
|
||||
However, we sample measurements in a discrete-time fashion, necessitating
|
||||
the need to convert the covariances to their discrete time equivalents.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
The IMU is modeled as a first order Gauss-Markov process, with a measurement
|
||||
noise and a process noise.
|
||||
Following
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Alg. 1 Page 57"
|
||||
key "Nikolic16thesis"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
and
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Eqns 129-130"
|
||||
key "Trawny05report_IndirectKF"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
, the measurement noises
|
||||
\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
|
||||
\end_inset
|
||||
|
||||
are simply scaled by
|
||||
\begin_inset Formula $\frac{1}{\Delta t}$
|
||||
\end_inset
|
||||
|
||||
, and the process noises
|
||||
\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
|
||||
\end_inset
|
||||
|
||||
are scaled by
|
||||
\begin_inset Formula $\Delta t$
|
||||
\end_inset
|
||||
|
||||
where
|
||||
\begin_inset Formula $\Delta t$
|
||||
\end_inset
|
||||
|
||||
is the time interval between 2 consecutive samples.
|
||||
For a thorough explanation of the discretization process, please refer
|
||||
to
|
||||
\begin_inset CommandInset citation
|
||||
LatexCommand cite
|
||||
after "Section 8.1"
|
||||
key "Simon06book"
|
||||
literal "false"
|
||||
|
||||
\end_inset
|
||||
|
||||
.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_inset CommandInset bibtex
|
||||
LatexCommand bibtex
|
||||
btprint "btPrintCited"
|
||||
bibfiles "refs"
|
||||
options "plain"
|
||||
|
||||
|
|
|
|||
Binary file not shown.
88
doc/refs.bib
88
doc/refs.bib
|
|
@ -1,26 +1,72 @@
|
|||
%% 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}}
|
||||
|
||||
@phdthesis{Nikolic16thesis,
|
||||
title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
|
||||
author={Nikolic, Janosch},
|
||||
year={2016},
|
||||
school={ETH Zurich}
|
||||
}
|
||||
|
||||
@book{Simon06book,
|
||||
title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
|
||||
author={Simon, Dan},
|
||||
year={2006},
|
||||
publisher={John Wiley \& Sons}
|
||||
}
|
||||
|
||||
@inproceedings{Trawny05report_IndirectKF,
|
||||
title={Indirect Kalman Filter for 3 D Attitude Estimation},
|
||||
author={Nikolas Trawny and Stergios I. Roumeliotis},
|
||||
year={2005}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -107,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
Matrix66 bias_acc_omega_init =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
|
|
@ -123,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
p->biasAccOmegaInit = bias_acc_omega_init;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
I_3x3 * 1e-8; // error committed in integrating position from velocities
|
||||
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
|
||||
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
|
||||
Matrix66 bias_acc_omega_int =
|
||||
Matrix66 bias_acc_omega_init =
|
||||
I_6x6 * 1e-5; // error in the bias used for preintegration
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
|
||||
|
|
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
|
|||
// PreintegrationCombinedMeasurements params:
|
||||
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
|
||||
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
|
||||
p->biasAccOmegaInt = bias_acc_omega_int;
|
||||
p->biasAccOmegaInit = bias_acc_omega_init;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
|
|||
GTSAM_MAKE_VECTOR_DEFS(10)
|
||||
GTSAM_MAKE_VECTOR_DEFS(11)
|
||||
GTSAM_MAKE_VECTOR_DEFS(12)
|
||||
GTSAM_MAKE_VECTOR_DEFS(15)
|
||||
|
||||
typedef Eigen::VectorBlock<Vector> SubVector;
|
||||
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const {
|
|||
<< endl;
|
||||
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
|
||||
<< endl;
|
||||
cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
|
||||
cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]"
|
||||
<< endl;
|
||||
}
|
||||
|
||||
|
|
@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth
|
|||
tol) &&
|
||||
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
|
||||
tol) &&
|
||||
equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
|
||||
equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -114,6 +114,10 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
|
||||
Matrix3 vel_H_biasAcc = B.bottomRows<3>();
|
||||
|
||||
Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
|
||||
Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
|
||||
Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Eigen::Matrix<double, 15, 15> F;
|
||||
F.setZero();
|
||||
|
|
@ -123,41 +127,61 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
F.block<3, 3>(6, 9) = vel_H_biasAcc;
|
||||
F.block<6, 6>(9, 9) = I_6x6;
|
||||
|
||||
// Update the uncertainty on the state (matrix F in [4]).
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
|
||||
|
||||
// propagate uncertainty
|
||||
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
|
||||
const Matrix3& aCov = p().accelerometerCovariance;
|
||||
const Matrix3& wCov = p().gyroscopeCovariance;
|
||||
const Matrix3& iCov = p().integrationCovariance;
|
||||
const Matrix6& bInitCov = p().biasAccOmegaInit;
|
||||
|
||||
// first order uncertainty propagation
|
||||
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
|
||||
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
|
||||
G_measCov_Gt.setZero(15, 15);
|
||||
|
||||
Matrix3 aCov_updated = (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0));
|
||||
const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
|
||||
const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
|
||||
const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
|
||||
const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
|
||||
|
||||
// BLOCK DIAGONAL TERMS
|
||||
D_t_t(&G_measCov_Gt) = ((1 / dt) * pos_H_biasAcc
|
||||
* aCov_updated
|
||||
* (pos_H_biasAcc.transpose())) + (dt * iCov);
|
||||
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
|
||||
* aCov_updated
|
||||
* (vel_H_biasAcc.transpose());
|
||||
D_R_R(&G_measCov_Gt) =
|
||||
(theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) //
|
||||
+
|
||||
(theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
|
||||
|
||||
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
|
||||
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
|
||||
* (theta_H_biasOmega.transpose());
|
||||
D_t_t(&G_measCov_Gt) =
|
||||
(pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) //
|
||||
+ (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
|
||||
+ (dt * iCov);
|
||||
|
||||
D_v_v(&G_measCov_Gt) =
|
||||
(vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) //
|
||||
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
|
||||
|
||||
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
|
||||
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
|
||||
|
||||
// OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
|
||||
* theta_H_biasOmega.transpose();
|
||||
D_v_R(&G_measCov_Gt) = temp;
|
||||
D_R_v(&G_measCov_Gt) = temp.transpose();
|
||||
D_R_t(&G_measCov_Gt) =
|
||||
theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
|
||||
D_R_v(&G_measCov_Gt) =
|
||||
theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
|
||||
D_t_R(&G_measCov_Gt) =
|
||||
pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
|
||||
D_t_v(&G_measCov_Gt) =
|
||||
(pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
|
||||
(pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
|
||||
D_v_R(&G_measCov_Gt) =
|
||||
vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
|
||||
D_v_t(&G_measCov_Gt) =
|
||||
(vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
|
||||
(vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
|
||||
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
preintMeasCov_.noalias() += G_measCov_Gt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -265,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
|
|||
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
|
||||
return os;
|
||||
}
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -62,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType;
|
|||
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
||||
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
||||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
|
||||
Matrix6 biasAccOmegaInit; ///< covariance of bias used as initial estimate.
|
||||
|
||||
/// Default constructor makes uninitialized params struct.
|
||||
/// Used for serialization.
|
||||
PreintegrationCombinedParams()
|
||||
: biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3),
|
||||
biasAccOmegaInt(I_6x6) {}
|
||||
biasAccOmegaInit(I_6x6) {}
|
||||
|
||||
/// See two named constructors below for good values of n_gravity in body frame
|
||||
PreintegrationCombinedParams(const Vector3& n_gravity) :
|
||||
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
|
||||
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
|
||||
biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
|
|||
|
||||
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
|
||||
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
|
||||
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
|
||||
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; }
|
||||
|
||||
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
|
||||
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
|
||||
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
|
||||
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; }
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -109,7 +109,7 @@ private:
|
|||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
|
|||
state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
|
||||
H1 || H3 ? &D_error_predict : 0);
|
||||
|
||||
if (H1) *H1 << D_error_predict* D_predict_state_i;
|
||||
if (H1) *H1 << D_error_predict * D_predict_state_i;
|
||||
if (H2) *H2 << D_error_state_j;
|
||||
if (H3) *H3 << D_error_predict* D_predict_bias_i;
|
||||
if (H3) *H3 << D_error_predict * D_predict_bias_i;
|
||||
|
||||
return error;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,8 +15,8 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
#include <cmath>
|
||||
|
|
@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
|
|||
return Q / (N - 1);
|
||||
}
|
||||
|
||||
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
|
||||
double T, const Bias& estimatedBias, bool corrupted) const {
|
||||
gttic_(integrate);
|
||||
PreintegratedCombinedMeasurements pim(p_, estimatedBias);
|
||||
|
||||
const double dt = imuSampleTime();
|
||||
const size_t nrSteps = T / dt;
|
||||
double t = 0;
|
||||
for (size_t k = 0; k < nrSteps; k++, t += dt) {
|
||||
Vector3 measuredOmega =
|
||||
corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
|
||||
Vector3 measuredAcc =
|
||||
corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||
}
|
||||
|
||||
return pim;
|
||||
}
|
||||
|
||||
NavState CombinedScenarioRunner::predict(
|
||||
const PreintegratedCombinedMeasurements& pim,
|
||||
const Bias& estimatedBias) const {
|
||||
const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
|
||||
return pim.predict(state_i, estimatedBias);
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
|
||||
double T, size_t N, const Bias& estimatedBias) const {
|
||||
gttic_(estimateCovariance);
|
||||
|
||||
// Get predict prediction from ground truth measurements
|
||||
NavState prediction = predict(integrate(T));
|
||||
|
||||
// Sample !
|
||||
Matrix samples(15, N);
|
||||
Vector15 sum = Vector15::Zero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
auto pim = integrate(T, estimatedBias, true);
|
||||
NavState sampled = predict(pim);
|
||||
Vector15 xi = Vector15::Zero();
|
||||
xi << sampled.localCoordinates(prediction),
|
||||
(estimatedBias_.vector() - estimatedBias.vector());
|
||||
samples.col(i) = xi;
|
||||
sum += xi;
|
||||
}
|
||||
|
||||
// Compute MC covariance
|
||||
Vector15 sampleMean = sum / N;
|
||||
Eigen::Matrix<double, 15, 15> Q;
|
||||
Q.setZero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Vector15 xi = samples.col(i) - sampleMean;
|
||||
Q += xi * xi.transpose();
|
||||
}
|
||||
|
||||
return Q / (N - 1);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,9 +16,10 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
|
|||
// also, uses g=10 for easy debugging
|
||||
const Vector3& gravity_n() const { return p_->n_gravity; }
|
||||
|
||||
const Scenario& scenario() const { return scenario_; }
|
||||
|
||||
// A gyro simply measures angular velocity in body frame
|
||||
Vector3 actualAngularVelocity(double t) const {
|
||||
return scenario_.omega_b(t);
|
||||
}
|
||||
Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
|
||||
|
||||
// An accelerometer measures acceleration in body, but not gravity
|
||||
Vector3 actualSpecificForce(double t) const {
|
||||
|
|
@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
|
|||
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
|
||||
};
|
||||
|
||||
/*
|
||||
* Simple class to test navigation scenarios with CombinedImuMeasurements.
|
||||
* Takes a trajectory scenario as input, and can generate IMU measurements
|
||||
*/
|
||||
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
|
||||
public:
|
||||
typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
|
||||
|
||||
private:
|
||||
const SharedParams p_;
|
||||
const Bias estimatedBias_;
|
||||
|
||||
public:
|
||||
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
|
||||
double imuSampleTime = 1.0 / 100.0,
|
||||
const Bias& bias = Bias())
|
||||
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
|
||||
imuSampleTime, bias),
|
||||
p_(p),
|
||||
estimatedBias_(bias) {}
|
||||
|
||||
/// Integrate measurements for T seconds into a PIM
|
||||
PreintegratedCombinedMeasurements integrate(
|
||||
double T, const Bias& estimatedBias = Bias(),
|
||||
bool corrupted = false) const;
|
||||
|
||||
/// Predict predict given a PIM
|
||||
NavState predict(const PreintegratedCombinedMeasurements& pim,
|
||||
const Bias& estimatedBias = Bias()) const;
|
||||
|
||||
/// Compute a Monte Carlo estimate of the predict covariance using N samples
|
||||
Eigen::Matrix<double, 15, 15> estimateCovariance(
|
||||
double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,18 +16,19 @@
|
|||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @author Stephen Williams
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <list>
|
||||
|
||||
|
|
@ -40,12 +41,15 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
|||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
p->biasAccCovariance = Z_3x3;
|
||||
p->biasOmegaCovariance = Z_3x3;
|
||||
p->biasAccOmegaInit = Z_6x6;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
} // namespace testing
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
||||
TEST(CombinedImuFactor, PreintegratedMeasurements ) {
|
||||
// Linearization point
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
|
||||
|
||||
|
|
@ -71,8 +75,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
|||
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, ErrorWithBiases ) {
|
||||
TEST(CombinedImuFactor, ErrorWithBiases ) {
|
||||
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
|
||||
|
|
@ -225,26 +230,91 @@ TEST(CombinedImuFactor, CheckCovariance) {
|
|||
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
Eigen::Matrix<double, 15, 15> expected;
|
||||
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
|
||||
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
|
||||
|
||||
// regression
|
||||
EXPECT(assert_equal(expected, actual.preintMeasCov()));
|
||||
}
|
||||
|
||||
// Test that the covariance values for the ImuFactor and the CombinedImuFactor
|
||||
// (top-left 9x9) are the same
|
||||
TEST(CombinedImuFactor, SameCovariance) {
|
||||
// IMU measurements and time delta
|
||||
Vector3 accMeas(0.1577, -0.8251, 9.6111);
|
||||
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
|
||||
double deltaT = 0.01;
|
||||
|
||||
// Assume zero bias
|
||||
imuBias::ConstantBias currentBias;
|
||||
|
||||
// Define params for ImuFactor
|
||||
auto params = PreintegrationParams::MakeSharedU();
|
||||
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
|
||||
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
|
||||
params->setIntegrationCovariance(pow(0, 2) * I_3x3);
|
||||
params->setOmegaCoriolis(Vector3::Zero());
|
||||
|
||||
// The IMU preintegration object for ImuFactor
|
||||
PreintegratedImuMeasurements pim(params, currentBias);
|
||||
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
// Define params for CombinedImuFactor
|
||||
auto combined_params = PreintegrationCombinedParams::MakeSharedU();
|
||||
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
|
||||
combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
|
||||
combined_params->setIntegrationCovariance(pow(0, 2) * I_3x3);
|
||||
combined_params->setOmegaCoriolis(Vector3::Zero());
|
||||
// Set bias integration covariance explicitly to zero
|
||||
combined_params->setBiasAccOmegaInt(Z_6x6);
|
||||
|
||||
// The IMU preintegration object for CombinedImuFactor
|
||||
PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
|
||||
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
// Assert if the noise covariance
|
||||
EXPECT(assert_equal(pim.preintMeasCov(),
|
||||
cpim.preintMeasCov().block(0, 0, 9, 9)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, Accelerating) {
|
||||
const double a = 0.2, v = 50;
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 initial_position(10, 20, 0);
|
||||
const Vector3 initial_velocity(v, 0, 0);
|
||||
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
|
||||
CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
|
||||
|
||||
PreintegratedCombinedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
auto estimatedCov = runner.estimateCovariance(T, 100);
|
||||
Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
|
||||
EXPECT(assert_equal(estimatedCov, expected, 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@ struct IMUHelper {
|
|||
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
|
||||
p->biasOmegaCovariance =
|
||||
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
|
||||
p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
|
||||
p->biasAccOmegaInit = Matrix::Identity(6, 6) * 1e-5;
|
||||
|
||||
// body to IMU rotation
|
||||
Rot3 iRb(0.036129, -0.998727, 0.035207,
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
|
|||
double gyrNoiseSigma = 0.000208;
|
||||
double gyrBiasRwSigma = 0.000004;
|
||||
double integrationCovariance = 1e-8;
|
||||
double biasAccOmegaInt = 1e-5;
|
||||
double biasAccOmegaInit = 1e-5;
|
||||
|
||||
double gravity = 9.81;
|
||||
double rate = 400.0; // Hz
|
||||
|
|
@ -76,7 +76,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
|
|||
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
|
||||
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
|
||||
imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
|
||||
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
|
||||
imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit;
|
||||
|
||||
// Initial state
|
||||
Pose3 priorPose;
|
||||
|
|
|
|||
Loading…
Reference in New Issue