12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Varun Agrawal 2022-07-04 22:06:39 -04:00 committed by GitHub
parent 63e2a593d6 bbd1e3f07a
commit 4244345677
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 203 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
doc/ImuFactor.lyx
View File

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

BIN
doc/ImuFactor.pdf
View File

Binary file not shown.

4
examples/CombinedImuFactorsExample.cpp
View File

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

4
examples/ImuFactorsExample.cpp
View File

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

59
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -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_R_R(&G_measCov_Gt) = theta_H_biasOmega //
* (wCov_updated / dt) //
* (theta_H_biasOmega.transpose());
D_t_t(&G_measCov_Gt) =
(pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) //
+ (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
+ (dt * iCov);
D_v_v(&G_measCov_Gt) =
(vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) //
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_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

12
gtsam/navigation/CombinedImuFactor.h
View File

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

4
gtsam/navigation/PreintegrationBase.cpp
View File

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

3
gtsam/navigation/ScenarioRunner.cpp
View File

@ -145,7 +145,8 @@ Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
auto pim = integrate(T, estimatedBias, true);
NavState sampled = predict(pim);
Vector15 xi = Vector15::Zero();
xi << sampled.localCoordinates(prediction), estimatedBias_.vector();
xi << sampled.localCoordinates(prediction),
(estimatedBias_.vector() - estimatedBias.vector());
samples.col(i) = xi;
sum += xi;
}

28
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -230,20 +230,20 @@ TEST(CombinedImuFactor, CheckCovariance) {
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Eigen::Matrix<double, 15, 15> expected;
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
// regression

2
gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
View File

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

4
tests/testImuPreintegration.cpp
View File

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