Added flag for second order coriolis terms (default is false)
djensen3
parent
eaf298bd18
commit
93458eaeff
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file CombinedImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -324,6 +324,8 @@ namespace gtsam {
|
|||
Vector3 omegaCoriolis_;
|
||||
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
|
||||
|
||||
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
|
|
@ -339,12 +341,13 @@ namespace gtsam {
|
|||
/** Constructor */
|
||||
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
|
||||
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
|
||||
Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor) {
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
virtual ~CombinedImuFactor() {}
|
||||
|
|
@ -435,19 +438,52 @@ namespace gtsam {
|
|||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
/*
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
- Rot_i.matrix(),
|
||||
- Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
*/
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
|
|
@ -578,7 +614,8 @@ namespace gtsam {
|
|||
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij;
|
||||
|
|
@ -590,7 +627,7 @@ namespace gtsam {
|
|||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
|
|
@ -603,6 +640,11 @@ namespace gtsam {
|
|||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file ImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -286,6 +286,8 @@ namespace gtsam {
|
|||
Vector3 omegaCoriolis_;
|
||||
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
|
||||
|
||||
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
|
|
@ -301,12 +303,13 @@ namespace gtsam {
|
|||
/** Constructor */
|
||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none) :
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor) {
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
virtual ~ImuFactor() {}
|
||||
|
|
@ -397,17 +400,29 @@ namespace gtsam {
|
|||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
- Rot_i.matrix(),
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
Matrix3::Zero(),
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
|
|
@ -497,7 +512,8 @@ namespace gtsam {
|
|||
/** predicted states from IMU */
|
||||
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
|
||||
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij;
|
||||
|
|
@ -509,7 +525,7 @@ namespace gtsam {
|
|||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
|
|
@ -522,6 +538,11 @@ namespace gtsam {
|
|||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
|
|||
Loading…
Reference in New Issue