diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index 0922a3e9c7..c335e69496 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -1,7 +1,9 @@ -#LyX 2.0 created this file. For more info see http://www.lyx.org/ -\lyxformat 413 +#LyX 2.3 created this file. For more info see http://www.lyx.org/ +\lyxformat 544 \begin_document \begin_header +\save_transient_properties true +\origin unavailable \textclass article \use_default_options true \maintain_unincluded_children false @@ -9,16 +11,18 @@ \language_package default \inputencoding auto \fontencoding global -\font_roman default -\font_sans default -\font_typewriter default +\font_roman "default" "default" +\font_sans "default" "default" +\font_typewriter "default" "default" +\font_math "auto" "auto" \font_default_family default \use_non_tex_fonts false \font_sc false \font_osf false -\font_sf_scale 100 -\font_tt_scale 100 - +\font_sf_scale 100 100 +\font_tt_scale 100 100 +\use_microtype false +\use_dash_ligatures true \graphics default \default_output_format default \output_sync 0 @@ -29,16 +33,26 @@ \use_hyperref false \papersize default \use_geometry true -\use_amsmath 1 -\use_esint 1 -\use_mhchem 1 -\use_mathdots 1 +\use_package amsmath 1 +\use_package amssymb 1 +\use_package cancel 1 +\use_package esint 1 +\use_package mathdots 1 +\use_package mathtools 1 +\use_package mhchem 1 +\use_package stackrel 1 +\use_package stmaryrd 1 +\use_package undertilde 1 \cite_engine basic +\cite_engine_type default +\biblio_style plain \use_bibtopic false \use_indices false \paperorientation portrait \suppress_date false +\justification true \use_refstyle 1 +\use_minted 0 \index Index \shortcut idx \color #008000 @@ -51,7 +65,10 @@ \tocdepth 3 \paragraph_separation indent \paragraph_indentation default -\quotes_language english +\is_math_indent 0 +\math_numbering_side default +\quotes_style english +\dynamic_quotes 0 \papercolumns 1 \papersides 1 \paperpagestyle default @@ -65,11 +82,11 @@ \begin_body \begin_layout Title -The new IMU Factor +The New IMU Factor \end_layout \begin_layout Author -Frank Dellaert +Frank Dellaert & Varun Agrawal \end_layout \begin_layout Standard @@ -91,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 @@ -244,7 +537,7 @@ X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\} then the differential equation describing the trajectory is \begin_inset Formula \[ -\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\, X(0)=\left\{ R_{0},P_{0},V_{0}\right\} +\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\} \] \end_inset @@ -285,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 @@ -592,6 +893,7 @@ Lie Group Methods \begin_inset CommandInset citation LatexCommand cite key "Iserles00an" +literal "true" \end_inset @@ -602,7 +904,7 @@ key "Iserles00an" , \begin_inset Formula \begin{equation} -\dot{R}(t)=F(R,t),\,\,\,\, R(0)=R_{0}\label{eq:diffSo3} +\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3} \end{equation} \end_inset @@ -707,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. @@ -947,8 +1240,8 @@ Or, as another way to state this, if we solve the differential equations \begin_inset Formula \begin{eqnarray*} \dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\ -\dot{p}(t) & = & R_{0}^{T}\, V_{0}+v(t)\\ -\dot{v}(t) & = & R_{0}^{T}\, g+R_{b}^{0}(t)a^{b}(t) +\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\ +\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t) \end{eqnarray*} \end_inset @@ -1000,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 @@ -1015,7 +1316,7 @@ v(t)=v_{g}(t)+v_{a}(t) evolving as \begin_inset Formula \begin{eqnarray*} -\dot{v}_{g}(t) & = & R_{i}^{T}\, g\\ +\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\ \dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t) \end{eqnarray*} @@ -1041,7 +1342,7 @@ p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t) evolving as \begin_inset Formula \begin{eqnarray*} -\dot{p}_{i}(t) & = & R_{i}^{T}\, V_{i}\\ +\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\ \dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\ \dot{p}_{v}(t) & = & v_{a}(t) \end{eqnarray*} @@ -1057,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)\\ @@ -1077,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] @@ -1085,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 @@ -1096,7 +1407,7 @@ Predict the NavState from \begin_inset Formula \[ -X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\} +X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\,p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\,v_{a}(t_{ij})\right\} \] \end_inset @@ -1179,11 +1490,59 @@ 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 -Even when we assume uncorrelated noise on +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 @@ -1201,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 @@ -1239,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 @@ -1280,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 @@ -1295,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 @@ -1307,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 @@ -1357,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] \] @@ -1372,7 +1768,7 @@ B_{k}=\left[\begin{array}{c} 0_{3\times3}\\ R_{k}\frac{\Delta_{t}}{2}^{2}\\ R_{k}\Delta_{t} -\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c} +\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c} H(\theta_{k})^{-1}\Delta_{t}\\ 0_{3\times3}\\ 0_{3\times3} @@ -1384,9 +1780,393 @@ H(\theta_{k})^{-1}\Delta_{t}\\ \end_layout +\begin_layout Subsubsection* +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. +\end_layout + +\begin_layout Standard +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 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}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 + + is the +\begin_inset Formula $15\times15$ +\end_inset + + derivative of +\begin_inset Formula $f$ +\end_inset + + wrpt this new +\begin_inset Formula $\zeta$ +\end_inset + +, and +\begin_inset Formula $G_{k}$ +\end_inset + + 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 +\begin_inset Formula $A_{k}$ +\end_inset + +, thus we only have the jacobians wrpt the biases left to account for. +\end_layout + +\begin_layout Standard +Conveniently, the jacobians of the pose and velocity wrpt the biases are + already computed in the +\emph on +ImuFactor +\emph default +derivation as matrices +\begin_inset Formula $B_{k}$ +\end_inset + + and +\begin_inset Formula $C_{k}$ +\end_inset + +, while they are identity matrices wrpt the biases themselves. + Thus, we can easily plug-in the values from the previous section to give + us the final result +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +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] +\] + +\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}}^{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" diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index 0b13c1f594..1823cbc4a6 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/doc/PreintegratedIMUJacobians.pdf b/doc/PreintegratedIMUJacobians.pdf new file mode 100644 index 0000000000..02616ee072 Binary files /dev/null and b/doc/PreintegratedIMUJacobians.pdf differ diff --git a/doc/refs.bib b/doc/refs.bib index 414773483f..ec42fb0328 100644 --- a/doc/refs.bib +++ b/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} -} \ No newline at end of file + 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} +} diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 9211a4d5f0..e0396ee818 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -60,13 +60,14 @@ namespace po = boost::program_options; po::variables_map parseOptions(int argc, char* argv[]) { po::options_description desc; - desc.add_options()("help,h", "produce help message")( - "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), - "path to the CSV file with the IMU data")( - "output_filename", - po::value()->default_value("imuFactorExampleResults.csv"), - "path to the result file to use")("use_isam", po::bool_switch(), - "use ISAM as the optimizer"); + desc.add_options()("help,h", "produce help message") // help message + ("data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data") // path to the data file + ("output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use") // filename to save results to + ("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); // flag for ISAM optimizer po::variables_map vm; po::store(po::parse_command_line(argc, argv, desc), vm); @@ -106,7 +107,7 @@ boost::shared_ptr 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); @@ -122,7 +123,7 @@ boost::shared_ptr 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->biasAccOmegaInt = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 38ee4c7c76..c176318642 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -94,7 +94,7 @@ boost::shared_ptr 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 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->biasAccOmegaInt = bias_acc_omega_init; return p; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 9cb2aa1650..f7923ff88c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -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 SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 3fe2cf4d16..8d3a7dd315 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + if (dt <= 0) { + throw std::runtime_error( + "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0"); + } + // Update preintegrated measurements. - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 B, C; + Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx) + Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively. PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first @@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - // TODO(frank): should we not also account for bias on position? - Matrix3 theta_H_biasOmega = -C.topRows<3>(); - Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); + Matrix3 theta_H_biasOmega = C.topRows<3>(); + 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 F; F.setZero(); F.block<9, 9>(0, 0) = A; F.block<3, 3>(0, 12) = theta_H_biasOmega; + F.block<3, 3>(3, 9) = pos_H_biasAcc; 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().biasAccOmegaInt; // first order uncertainty propagation - // Optimized matrix multiplication (1/dt) * G * measurementCovariance * - // G.transpose() + // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); + 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) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc - * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) - * (vel_H_biasAcc.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_R_R(&G_measCov_Gt) = + (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) // + + + (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.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(); - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; + 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_.noalias() += G_measCov_Gt; } //------------------------------------------------------------------------------ @@ -253,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 diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 213f5f223f..b496181eda 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType; * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * Available in this repo as "PreintegratedIMUJacobians.pdf". * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation, * Robotics: Science and Systems (RSS), 2015. @@ -61,7 +62,7 @@ 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 biasAccOmegaInt; ///< covariance of bias used as initial estimate. /// Default constructor makes uninitialized params struct. /// Used for serialization. @@ -92,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 setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 28c0461b1d..9b6affaaf8 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 B, C; + Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively. PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: @@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( const Matrix3& iCov = p().integrationCovariance; // (1/dt) allows to pass from continuous time noise to discrete time noise + // Update the uncertainty on the state (matrix A in [4]). preintMeasCov_ = A * preintMeasCov_ * A.transpose(); + // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]). preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); - // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 + // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix) preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7408271627..e89c2afb5d 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType; * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * Available in this repo as "PreintegratedIMUJacobians.pdf". * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", * Robotics: Science and Systems (RSS), 2015. diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 8840c34e9d..f6e9fccb8d 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -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; } diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 3938ce86c4..9d3e258de9 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -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 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 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 diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 1577e36fe1..cee5a54ab6 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,9 +16,10 @@ */ #pragma once +#include +#include #include #include -#include 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 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(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 estimateCovariance( + double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const; +}; + } // namespace gtsam diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 6ede1645f3..ab95d5a292 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { void setBiasAccCovariance(Matrix cov); void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); + void setBiasAccOmegaInit(Matrix cov); Matrix getBiasAccCovariance() const ; Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; + Matrix getBiasAccOmegaInit() const; }; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 2bbc2cc7c7..aacfff0f0d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -16,18 +16,19 @@ * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams + * @author Varun Agrawal */ -#include +#include +#include +#include +#include +#include #include #include -#include +#include +#include #include -#include -#include -#include - -#include #include @@ -40,12 +41,15 @@ static boost::shared_ptr 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->biasAccOmegaInt = 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)); @@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) { EXPECT(assert_equal(expectedPose, actual.pose(), tol)); } +/* ************************************************************************* */ +// Testing covariance to check if all the jacobians are accounted for. +TEST(CombinedImuFactor, CheckCovariance) { + auto params = PreintegrationCombinedParams::MakeSharedU(9.81); + + params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); + params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); + params->setIntegrationCovariance(pow(0.0, 2) * I_3x3); + params->setOmegaCoriolis(Vector3::Zero()); + + imuBias::ConstantBias currentBias; + + PreintegratedCombinedMeasurements actual(params, currentBias); + + // Measurements + Vector3 measuredAcc(0.1577, -0.8251, 9.6111); + Vector3 measuredOmega(-0.0210, 0.0311, 0.0145); + double deltaT = 0.01; + + actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + Eigen::Matrix 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, 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); + // Set bias integration covariance explicitly to zero + combined_params->setIntegrationCovariance(Z_3x3); + combined_params->setOmegaCoriolis(Z_3x1); + // Set bias initial covariance explicitly to zero + combined_params->setBiasAccOmegaInit(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 expected = pim.preintMeasCov(); + EXPECT(assert_equal(estimatedCov, expected, 0.1)); +} + /* ************************************************************************* */ int main() { TestResult tr;