Merge pull request #879 from borglab/fix/combined-imu-cov

Fix Preintegration Covariance of CombinedImuFactor

doc/ImuFactor.lyx

@@ -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.
+The recipe for the IMU factor is then, in summary:
- Solve the ordinary differential equations
+\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 
+, and 
-\begin_inset Formula $15\times15$
+\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}
+F_{k}\approx\left[\begin{array}{ccccc}
-I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} &  &  &  & H(\theta_{k})^{-1}\Delta_{t}\\
+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}\\
+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} &  & I_{3\times3} & R_{k}\Delta_{t}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
- &  &  & I_{3\times3}\\
+0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
- &  &  &  & I_{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"
 

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},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
+	journal = {Acta Numerica 2000},
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
+	pages = {215--365},
-  journal =	 {Acta Numerica 2000},
+	publisher = {Cambridge Univ Press},
-  volume =	 {9},
+	title = {Lie-group methods},
-  pages =	 {215--365},
+	volume = {9},
-  year =	 {2000},
+	year = {2000}}
-  publisher =	 {Cambridge Univ Press}
-}
 
 @book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
+	publisher = {CRC press},
-                  Shankar and Sastry, S Shankara},
+	title = {A mathematical introduction to robotic manipulation},
-  year =	 {1994},
+	year = {1994}}
-  publisher =	 {CRC press}
-}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
+	author = {Spivak, Michael},
-  author =	 {Spivak, Michael},
+	publisher = {WA Benjamin New York},
-  volume =	 {1},
+	title = {Calculus on manifolds},
-  year =	 {1965},
+	volume = {1},
-  publisher =	 {WA Benjamin New York}
+	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}
+}

examples/CombinedImuFactorsExample.cpp

@@ -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;
 }

examples/ImuFactorsExample.cpp

@@ -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;
 }

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<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;

gtsam/navigation/CombinedImuFactor.cpp

@@ -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
+  D_R_R(&G_measCov_Gt) =
-      * aCov_updated
+      (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose())  //
-      * (pos_H_biasAcc.transpose())) + (dt * iCov);
+      +
-  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
+      (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
-      * aCov_updated
-      * (vel_H_biasAcc.transpose());
 
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
+  D_t_t(&G_measCov_Gt) =
-      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
+      (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose())           //
-      * (theta_H_biasOmega.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)
+  D_R_t(&G_measCov_Gt) =
-      * theta_H_biasOmega.transpose();
+      theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
-  D_v_R(&G_measCov_Gt) = temp;
+  D_R_v(&G_measCov_Gt) =
-  D_R_v(&G_measCov_Gt) = temp.transpose();
+      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

gtsam/navigation/CombinedImuFactor.h

@@ -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:

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;
 }

gtsam/navigation/ScenarioRunner.cpp

@@ -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

gtsam/navigation/ScenarioRunner.h

@@ -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 {
+  Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
-    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

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -16,18 +16,19 @@
  * @author  Frank Dellaert
  * @author  Richard Roberts
  * @author  Stephen Williams
+ * @author  Varun Agrawal
  */
 
-#include <gtsam/navigation/ImuFactor.h>
+#include <CppUnitLite/TestHarness.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 <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-
+#include <gtsam/geometry/Pose3.h>
-#include <CppUnitLite/TestHarness.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,  //
+  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.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.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, 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, 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, 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, 0.010001, 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.010001, 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.010001, 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.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.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.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.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.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;

gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp

@@ -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,

tests/testImuPreintegration.cpp

@@ -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;