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

added additional coriolis terms

release/4.3a0
djensen 2014-02-17 09:36:37 -05:00
parent 4ef8cec118
commit c8c728ad6f
1 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/navigation/CombinedImuFactor.h
View File

@ -452,12 +452,12 @@ namespace gtsam {
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi // dfP/dPi
- Rot_i.matrix(), - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi // dfV/dPi
Matrix3::Zero(), skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
// dfR/dRi // dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi // dfR/dPi
@ -476,14 +476,13 @@ namespace gtsam {
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi // dfV/dVi
- Matrix3::Identity() - Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi // dfR/dVi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dVi //dBiasAcc/dVi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dVi //dBiasOmega/dVi
Matrix3::Zero(); Matrix3::Zero();
} }
if(H3) { if(H3) {
@ -564,6 +563,7 @@ namespace gtsam {
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- vel_i * deltaTij - vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij*deltaTij
- 0.5 * gravity_ * deltaTij*deltaTij; - 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv = const Vector3 fv =
@ -571,6 +571,7 @@ namespace gtsam {
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
+ skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij // Coriolis term
- gravity_ * deltaTij; - gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat); const Vector3 fR = Rot3::Logmap(fRhat);