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

coriolis now lives in NavState

release/4.3a0
dellaert 2015-07-21 20:35:33 -04:00
parent 4c177c0ce2
commit a9b4cdbe47
5 changed files with 199 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
gtsam/navigation/NavState.cpp
View File

@ -18,22 +18,27 @@
#include <gtsam/navigation/NavState.h>
using namespace std;
namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
if (H) *H << I_3x3, Z_3x3, Z_3x3;
if (H)
*H << I_3x3, Z_3x3, Z_3x3;
return R_;
}
const Point3& NavState::position(OptionalJacobian<3, 9> H) const {
if (H) *H << Z_3x3, R(), Z_3x3;
if (H)
*H << Z_3x3, R(), Z_3x3;
return t_;
}
const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const {
if (H) *H << Z_3x3, Z_3x3, R();
if (H)
*H << Z_3x3, Z_3x3, R();
return v_;
}
@ -44,7 +49,7 @@ Matrix7 NavState::matrix() const {
return T;
}
void NavState::print(const std::string& s) const {
void NavState::print(const string& s) const {
attitude().print(s + ".R");
position().print(s + ".p");
gtsam::print((Vector) v_, s + ".v");
@ -101,7 +106,7 @@ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
OptionalJacobian<9, 9> H) {
if (H)
throw std::runtime_error(
throw runtime_error(
"NavState::ChartOrigin::Local derivative not implemented yet");
Vector9 xi;
xi << Rot3::Logmap(x.R_), x.t(), x.v();
@ -110,7 +115,7 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
if (H)
throw std::runtime_error("NavState::Expmap derivative not implemented yet");
throw runtime_error("NavState::Expmap derivative not implemented yet");
Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
@ -119,7 +124,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
// NOTE(frank): See Pose3::Expmap
Rot3 nRb = Rot3::Expmap(n_omega_nb);
double theta2 = n_omega_nb.dot(n_omega_nb);
if (theta2 > std::numeric_limits<double>::epsilon()) {
if (theta2 > numeric_limits<double>::epsilon()) {
// Expmap implements a "screw" motion in the direction of n_omega_nb
Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
@ -136,7 +141,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
if (H)
throw std::runtime_error("NavState::Logmap derivative not implemented yet");
throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p.vector();
@ -145,7 +150,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
Vector9 xi;
Vector3 n_omega_nb = Rot3::Logmap(nRb);
double theta = n_omega_nb.norm();
if (theta * theta <= std::numeric_limits<double>::epsilon()) {
if (theta * theta <= numeric_limits<double>::epsilon()) {
xi << n_omega_nb, n_t, n_v;
} else {
Matrix3 W = skewSymmetric(n_omega_nb / theta);
@ -179,4 +184,45 @@ Matrix7 NavState::wedge(const Vector9& xi) {
return T;
}
// sugar for derivative blocks
#define D_R_R(H) H->block<3,3>(0,0)
#define D_t_t(H) H->block<3,3>(3,3)
#define D_t_v(H) H->block<3,3>(3,6)
#define D_v_t(H) H->block<3,3>(6,3)
#define D_v_v(H) H->block<3,3>(6,6)
Vector9 NavState::coriolis(const Vector3& omega, double dt, bool secondOrder,
OptionalJacobian<9, 9> H) const {
Vector9 result;
const Rot3& nRb = attitude();
const Point3& n_t = position(); // derivative is R() !
const Vector3& n_v = velocity(); // ditto
const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v);
Matrix3 D_dP_R;
dR(result) << -nRb.unrotate(omega * dt, H ? &D_dP_R : 0);
dP(result) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(result) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector()));
dP(result) -= (0.5 * dt2) * omega_cross2_t;
dV(result) -= dt * omega_cross2_t;
}
if (H) {
const Matrix3 Omega = skewSymmetric(omega);
const Matrix3 D_cross_state = Omega * R();
H->setZero();
D_R_R(H) << -D_dP_R;
D_t_v(H) << (-dt2) * D_cross_state;
D_v_v(H) << (-2.0 * dt) * D_cross_state;
if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= dt * D_cross2_state;
}
}
return result;
}
} /// namespace gtsam

8
gtsam/navigation/NavState.h
View File

@ -200,6 +200,14 @@ public:
static Matrix7 wedge(const Vector9& xi);
/// @}
/// @name Dynamics
/// @{
// Compute tangent space contribution due to coriolis forces
Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder,
OptionalJacobian<9, 9> H) const;
/// @}
private:
/// @{

193
gtsam/navigation/PreintegrationBase.cpp
View File

@ -46,23 +46,24 @@ void PreintegrationBase::print(const string& s) const {
}
/// Needed for testable
bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(other, tol) &&
biasHat_.equals(other.biasHat_, tol) &&
equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) &&
equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) &&
equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) &&
equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) &&
equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) &&
equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return PreintegratedRotation::equals(other, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
/// Update preintegrated measurements
void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
void PreintegrationBase::updatePreintegratedMeasurements(
const Vector3& correctedAcc, const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Vector3 temp = dRij * correctedAcc * deltaT;
if (!p().use2ndOrderIntegration) {
@ -74,7 +75,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte
Matrix3 R_i, F_angles_angles;
if (F)
R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if (F) {
@ -86,19 +87,20 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte
F_pos_angles = Z_3x3;
// pos vel angle
*F << //
I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
*F << //
I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
}
}
/// Update Jacobians to be used during preintegration
void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
void PreintegrationBase::updatePreintegratedJacobians(
const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega();
if (!p().use2ndOrderIntegration) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
@ -112,8 +114,8 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc
}
void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3* correctedAcc,
Vector3* correctedOmega) {
const Vector3& measuredAcc, const Vector3& measuredOmega,
Vector3* correctedAcc, Vector3* correctedOmega) {
*correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
*correctedOmega = biasHat_.correctGyroscope(measuredOmega);
@ -121,10 +123,11 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
// (originally in the IMU frame) into the body frame
if (p().body_P_sensor) {
Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix();
*correctedOmega = body_R_sensor * (*correctedOmega); // rotation rate vector in the body frame
*correctedOmega = body_R_sensor * (*correctedOmega); // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(*correctedOmega);
*correctedAcc = body_R_sensor * (*correctedAcc)
- body_omega_body__cross * body_omega_body__cross * p().body_P_sensor->translation().vector();
- body_omega_body__cross * body_omega_body__cross
* p().body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
}
@ -157,38 +160,14 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
OptionalJacobian<9, 9> H) const {
Vector9 result = Vector9::Zero();
if (H) H->setZero();
if (p().omegaCoriolis) {
const Rot3& rot_i = state_i.attitude();
const Point3& t_i = state_i.position();
const Vector3& vel_i = state_i.velocity();
const double dt = deltaTij(), dt2 = dt * dt;
const Vector3& omegaCoriolis = *p().omegaCoriolis;
Matrix3 D_dP_Ri;
NavState::dR(result) -= rot_i.unrotate(omegaCoriolis * dt, H ? &D_dP_Ri : 0);
NavState::dP(result) -= (dt2 * omegaCoriolis.cross(vel_i)); // NOTE(luca): we got rid of the 2 wrt INS paper
NavState::dV(result) -= ((2.0 * dt) * omegaCoriolis.cross(vel_i));
if (p().use2ndOrderCoriolis) {
Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(t_i.vector()));
NavState::dP(result) -= 0.5 * temp * dt2;
NavState::dV(result) -= temp * dt;
}
if (H) {
// Matrix3 Ri = rot_i.matrix();
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
H->block<3,3>(0,0) -= D_dP_Ri;
H->block<3,3>(3,6) -= omegaCoriolisHat * dt2;
H->block<3,3>(6,6) -= (2.0 * dt) * omegaCoriolisHat;
if (p().use2ndOrderCoriolis) {
const Matrix3 omegaCoriolisHat2 = omegaCoriolisHat * omegaCoriolisHat;
H->block<3,3>(3,3) -= 0.5 * omegaCoriolisHat2 * dt2;
H->block<3,3>(6,3) -= omegaCoriolisHat2 * dt;
}
}
return state_i.coriolis(*(p().omegaCoriolis), deltaTij(),
p().use2ndOrderCoriolis, H);
} else {
if (H)
H->setZero();
return Vector9::Zero();
}
return result;
}
//------------------------------------------------------------------------------
@ -204,8 +183,10 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
Vector9 delta;
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
NavState::dR(delta) = NavState::dR(biasCorrectedDelta);
NavState::dP(delta) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri, D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri, D_dV_bc) + p().gravity * dt;
NavState::dP(delta) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri,
D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri,
D_dV_bc) + p().gravity * dt;
Matrix9 Hcoriolis;
if (p().omegaCoriolis) {
@ -213,17 +194,18 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
}
if (H1) {
H1->setZero();
H1->block<3,3>(3,0) = D_dP_Ri;
H1->block<3,3>(3,6) = I_3x3 * dt;
H1->block<3,3>(6,0) = D_dV_Ri;
if (p().omegaCoriolis) *H1 += Hcoriolis;
H1->block<3, 3>(3, 0) = D_dP_Ri;
H1->block<3, 3>(3, 6) = I_3x3 * dt;
H1->block<3, 3>(6, 0) = D_dV_Ri;
if (p().omegaCoriolis)
*H1 += Hcoriolis;
}
if (H2) {
H2->setZero();
Matrix3 Ri = rot_i.matrix();
H2->block<3,3>(0,0) = I_3x3;
H2->block<3,3>(3,3) = Ri;
H2->block<3,3>(6,6) = Ri;
H2->block<3, 3>(0, 0) = I_3x3;
H2->block<3, 3>(3, 3) = Ri;
H2->block<3, 3>(6, 6) = Ri;
}
return delta;
@ -279,14 +261,11 @@ static Vector9 computeError(const NavState& state_i, const NavState& state_j,
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H1,
OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 6> H3,
OptionalJacobian<9, 3> H4,
OptionalJacobian<9, 6> H5) const {
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
// we give some shorter name to rotations and translations
const Rot3& rot_i = pose_i.rotation();
@ -305,11 +284,13 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
/// Predict state at time j
Matrix99 D_predict_state;
Matrix96 D_predict_bias;
NavState predictedState_j = predict(state_i, bias_i, D_predict_state, D_predict_bias);
NavState predictedState_j = predict(state_i, bias_i, D_predict_state,
D_predict_bias);
// Evaluate residual error, according to [3]
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose().translation().vector());
const Vector3 fp = Ri.transpose()
* (pos_j - predictedState_j.pose().translation().vector());
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity());
@ -324,66 +305,68 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
// Residual rotation error
Matrix3 D_cDeltaRij_cOmega;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, H1 || H5 ? &D_cDeltaRij_cOmega : 0);
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega,
H1 || H5 ? &D_cDeltaRij_cOmega : 0);
const Rot3 RiBetweenRj = rot_i.between(rot_j);
const Rot3 fRrot = correctedDeltaRij.between(RiBetweenRj);
Matrix3 D_fR_fRrot;
Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0);
const double dt = deltaTij(), dt2 = dt*dt;
const double dt = deltaTij(), dt2 = dt * dt;
Matrix3 RitOmegaCoriolisHat = Z_3x3;
if ((H1 || H2) && p().omegaCoriolis)
RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis);
RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis);
if (H1) {
const Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3;
if (p().use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * p().omegaCoriolisHat * p().omegaCoriolisHat * Ri
const Matrix3 temp = RitOmegaCoriolisHat * (-RitOmegaCoriolisHat.transpose());
const Matrix3 temp = RitOmegaCoriolisHat
* (-RitOmegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * dt2;
dfVdPi += temp * dt;
}
(*H1) <<
D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi
Z_3x3, // dfR/dPi
skewSymmetric(fp + NavState::dP(biasCorrected)), // dfP/dRi
dfPdPi, // dfP/dPi
skewSymmetric(fv + NavState::dV(biasCorrected)), // dfV/dRi
dfVdPi; // dfV/dPi
(*H1)
<< D_fR_fRrot
* (-rot_j.between(rot_i).matrix()
- fRrot.inverse().matrix() * D_coriolis), // dfR/dRi
Z_3x3, // dfR/dPi
skewSymmetric(fp + NavState::dP(biasCorrected)), // dfP/dRi
dfPdPi, // dfP/dPi
skewSymmetric(fv + NavState::dV(biasCorrected)), // dfV/dRi
dfVdPi; // dfV/dPi
}
if (H2) {
(*H2) <<
Z_3x3, // dfR/dVi
-Ri.transpose() * dt + RitOmegaCoriolisHat * dt2, // dfP/dVi
-Ri.transpose() + 2 * RitOmegaCoriolisHat * dt; // dfV/dVi
(*H2) << Z_3x3, // dfR/dVi
-Ri.transpose() * dt + RitOmegaCoriolisHat * dt2, // dfP/dVi
-Ri.transpose() + 2 * RitOmegaCoriolisHat * dt; // dfV/dVi
}
if (H3) {
(*H3) <<
D_fR_fRrot, Z_3x3, // dfR/dPosej
(*H3) << D_fR_fRrot, Z_3x3, // dfR/dPosej
Z_3x3, Ri.transpose() * rot_j.matrix(), // dfP/dPosej
Matrix::Zero(3, 6); // dfV/dPosej
Matrix::Zero(3, 6); // dfV/dPosej
}
if (H4) {
(*H4) <<
Z_3x3, // dfR/dVj
Z_3x3, // dfP/dVj
(*H4) << Z_3x3, // dfR/dVj
Z_3x3, // dfP/dVj
Ri.transpose(); // dfV/dVj
}
if (H5) {
const Matrix36 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.middleRows<3>(0);
(*H5) <<
-D_fR_fRrot * fRrot.inverse().matrix() * JbiasOmega, // dfR/dBias
-D_biasCorrected_bias.middleRows<3>(3), // dfP/dBias
-D_biasCorrected_bias.middleRows<3>(6); // dfV/dBias
const Matrix36 JbiasOmega = D_cDeltaRij_cOmega
* D_biasCorrected_bias.middleRows<3>(0);
(*H5) << -D_fR_fRrot * fRrot.inverse().matrix() * JbiasOmega, // dfR/dBias
-D_biasCorrected_bias.middleRows<3>(3), // dfP/dBias
-D_biasCorrected_bias.middleRows<3>(6); // dfV/dBias
}
// TODO(frank): Do everything via derivatives of function below
return computeError(state_i, state_j, predictedState_j);
}
//------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis,
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
@ -393,4 +376,4 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3&
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
} /// namespace gtsam
} /// namespace gtsam

6
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -282,7 +282,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
EXPECT(assert_equal(expectedH, actualH));
}
{
Matrix99 actualH;
Matrix9 actualH;
pim.integrateCoriolis(state1, actualH);
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&PreintegrationBase::integrateCoriolis, pim, _1,
@ -290,7 +290,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
EXPECT(assert_equal(expectedH, actualH));
}
{
Matrix99 aH1, aH2;
Matrix9 aH1, aH2;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
pim.recombinedPrediction(state1, biasCorrectedDelta, aH1, aH2);
Matrix eH1 = numericalDerivative11<Vector9, NavState>(
@ -303,7 +303,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
EXPECT(assert_equal(eH2, aH2));
}
{
Matrix99 aH1;
Matrix9 aH1;
Matrix96 aH2;
pim.predict(state1, bias, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>(

45
gtsam/navigation/tests/testNavState.cpp
View File

@ -141,6 +141,51 @@ TEST( NavState, Lie ) {
EXPECT(assert_equal(xi, -state3.logmap(state2)));
}
/* ************************************************************************* */
TEST(NavState, Coriolis) {
Matrix9 actualH;
Vector3 omegaCoriolis(0.02, 0.03, 0.04);
double dt = 0.5;
// first-order
bool secondOrder = false;
kState1.coriolis(omegaCoriolis, dt, secondOrder, actualH);
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder,
boost::none), kState1);
EXPECT(assert_equal(expectedH, actualH));
// second-order
secondOrder = true;
kState1.coriolis(omegaCoriolis, dt, secondOrder, actualH);
expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder,
boost::none), kState1);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST(NavState, Coriolis2) {
NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
Matrix9 actualH;
Vector3 omegaCoriolis(0.02, 0.03, 0.04);
double dt = 2.0;
// first-order
bool secondOrder = false;
state2.coriolis(omegaCoriolis, dt, secondOrder, actualH);
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder,
boost::none), state2);
EXPECT(assert_equal(expectedH, actualH));
// second-order
secondOrder = true;
state2.coriolis(omegaCoriolis, dt, secondOrder, actualH);
expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder,
boost::none), state2);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
int main() {
TestResult tr;