Merge pull request #882 from borglab/fix/combined-imu

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
@@ -227,16 +227,62 @@ preintegrated_
 
 \begin_layout Standard
 The main function of a factor is to calculate an error.
- The easiest case to look at is the NavState variant in ImuFactor2, which
- is given as:
+ This is done exactly the same in both variants: 
 \begin_inset Formula 
 \begin{equation}
-\Delta X_{ij}=X_{j}-\hat{X_{ij}}\label{eq:imu-factor-error}
+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*
@@ -263,6 +309,20 @@ 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
@@ -282,6 +342,14 @@ Gyroscope Covariance
 : 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}$
@@ -298,14 +366,6 @@ Accelerometer Bias Covariance
  : The covariance associated with the accelerometer bias random walk.
 \end_layout
 
-\begin_layout Itemize
-Gyroscope Bias Covariance 
-\begin_inset Formula $Q_{\Delta b^{\omega}}$
-\end_inset
-
- : The covariance associated with the gyroscope bias random walk.
-\end_layout
-
 \begin_layout Itemize
 Integration Covariance 
 \begin_inset Formula $Q_{int}$
@@ -518,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
 
@@ -1469,7 +1537,12 @@ 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
 
@@ -1487,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
@@ -1566,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
 
@@ -1593,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_{3\times3}-\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
@@ -1981,6 +2091,13 @@ which we can break into 3 matrices for clarity, representing the main diagonal
 
 \begin_layout Subsubsection*
 Covariance Discretization
+\begin_inset CommandInset label
+LatexCommand label
+name "subsec:Covariance-Discretization"
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Standard

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/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();
@@ -131,37 +135,51 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   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);
-  Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3);
+  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) = (pos_H_biasAcc  //
-      * (aCov_updated / dt)  //
-      * pos_H_biasAcc.transpose()) + (dt * iCov);
-  D_v_v(&G_measCov_Gt) = vel_H_biasAcc  //
-      * (aCov_updated / dt)  //
-      * (vel_H_biasAcc.transpose());
+  D_R_R(&G_measCov_Gt) =
+      (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose())  //
+      +
+      (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
+
+  D_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_R_R(&G_measCov_Gt) = theta_H_biasOmega  //
-      * (wCov_updated / dt)  //
-      * (theta_H_biasOmega.transpose());
+  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_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose();
-  D_R_v(&G_measCov_Gt) = temp.transpose();
-  D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose();
+  D_R_t(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
+  D_R_v(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
+  D_t_R(&G_measCov_Gt) =
+      pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_t_v(&G_measCov_Gt) =
+      (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
+      (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
+  D_v_R(&G_measCov_Gt) =
+      vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_v_t(&G_measCov_Gt) =
+      (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
+      (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
 
   preintMeasCov_.noalias() += G_measCov_Gt;
 }
@@ -271,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