From bc16edd2ac13b7aa6939d827570701c0873fccbf Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 19 Feb 2013 21:24:44 +0000 Subject: [PATCH] Added Equivalent IMU factor to gtsam_unstable --- gtsam_unstable/CMakeLists.txt | 2 + gtsam_unstable/navigation/CMakeLists.txt | 26 + .../EquivInertialNavFactor_GlobalVel.h | 677 ++++++++++++++++++ gtsam_unstable/navigation/ImuBias.h | 182 +++++ .../testEquivInertialNavFactor_GlobalVel.h | 67 ++ .../navigation/tests/testImuBias.cpp | 43 ++ 6 files changed, 997 insertions(+) create mode 100644 gtsam_unstable/navigation/CMakeLists.txt create mode 100644 gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h create mode 100644 gtsam_unstable/navigation/ImuBias.h create mode 100644 gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h create mode 100644 gtsam_unstable/navigation/tests/testImuBias.cpp diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index cd209ea95..575ee427f 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -8,6 +8,7 @@ set (gtsam_unstable_subdirs dynamics # nonlinear slam + navigation ) set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRARY}) @@ -40,6 +41,7 @@ set(gtsam_unstable_srcs #${linear_srcs} #${nonlinear_srcs} ${slam_srcs} + ${navigation_srcs} ) if(NOT MSVC) diff --git a/gtsam_unstable/navigation/CMakeLists.txt b/gtsam_unstable/navigation/CMakeLists.txt new file mode 100644 index 000000000..fcd458c07 --- /dev/null +++ b/gtsam_unstable/navigation/CMakeLists.txt @@ -0,0 +1,26 @@ +# Install headers +file(GLOB navigation_headers "*.h") +install(FILES ${navigation_headers} DESTINATION include/gtsam_unstable/navigation) + +# Components to link tests in this subfolder against +set(navigation_local_libs +# navigation_unstable + slam + nonlinear + linear + inference + geometry + base + ccolamd +) + +set (navigation_full_libs + gtsam-static + gtsam_unstable-static) + +# Exclude tests that don't work +set (navigation_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(navigation_unstable "${navigation_local_libs}" "${navigation_full_libs}" "${navigation_excluded_tests}") +add_dependencies(check.unstable check.navigation_unstable) diff --git a/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h new file mode 100644 index 000000000..910cc8d63 --- /dev/null +++ b/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h @@ -0,0 +1,677 @@ + +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EquivInertialNavFactor_GlobalVel.h + * @author Vadim Indelman, Stephen Williams + * @brief Equivalent inertial navigation factor (velocity in the global frame). + * @date Sep. 26, 2012 + **/ + +#pragma once + +#include +#include +#include +#include +#include + +// Using numerical derivative to calculate d(Pose3::Expmap)/dw +#include + +#include + +#include + +namespace gtsam { + +/* + * NOTES: + * ===== + * Concept: Based on [Lupton12tro] + * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations. + * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). + * All sensor-to-body transformations are performed here. + * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial. + * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will + * relate between navigation variables at the two time instances (t0 and current time). + * + * Other notes: + * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame. + * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames. + * - Camera and IMU frames are identical + * - The user should specify a continuous equivalent noise covariance, which can be calculated using + * the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance + * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a + * discrete form using the supplied delta_t between sub-sequential measurements. + * - Earth-rate correction: + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + * - Frame Notation: + * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} + * So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor + * And the transformation from the body frame to the world frame would be: world_P_body + * This allows visual chaining. For example, converting the sensed angular velocity of the IMU + * (angular velocity of the sensor in the sensor frame) into the world frame can be performed as: + * world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor + * + * + * - Common Quantity Types + * P : pose/3d transformation + * R : rotation + * omega : angular velocity + * t : translation + * v : velocity + * a : acceleration + * + * - Common Frames + * sensor : the coordinate system attached to the sensor origin + * body : the coordinate system attached to body/inertial frame. + * Unless an optional frame transformation is provided, the + * sensor frame and the body frame will be identical + * world : the global/world coordinate frame. This is assumed to be + * a tangent plane to the earth's surface somewhere near the + * vehicle + */ + +template +class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 { + +private: + + typedef EquivInertialNavFactor_GlobalVelocity This; + typedef NoiseModelFactor5 Base; + + Vector delta_pos_in_t0_; + Vector delta_vel_in_t0_; + Vector3 delta_angles_; + double dt12_; + + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; + + Matrix Jacobian_wrt_t0_Overall_; + + boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements + boost::optional body_P_sensor_; // The pose of the sensor in the body frame + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + EquivInertialNavFactor_GlobalVel() {} + + /** Constructor */ + EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, + const Matrix& Jacobian_wrt_t0_Overall, + boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : + Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), + delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), + dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), + Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { } + + virtual ~EquivInertialNavFactor_GlobalVel() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\n"; + std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; + std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; + std::cout << "delta_angles: " << this->delta_angles_ << std::endl; + std::cout << "dt12: " << this->dt12_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + this->noiseModel_->print(" noise model"); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol + && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol + && (delta_angles_ - e->delta_angles_).norm() < tol + && (dt12_ - e->dt12_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + + POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { + + // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.bias_acc(); + Vector delta_BiasGyro = Bias1.bias_gyro(); + if (Bias_initial_){ + delta_BiasAcc -= Bias_initial_->bias_acc(); + delta_BiasGyro -= Bias_initial_->bias_gyro(); + } + + Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); + Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3); + Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3); + + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; + + /* Rotation term */ + Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro; + // Another alternative: + // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); + + return predictPose_inertial(Pose1, Vel1, + delta_pos_in_t0_corrected, delta_angles_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } + + static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ + + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; + + /* Position term */ + Vector body_deltaPos_body = delta_pos_in_t0; + + Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; + Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; + + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; + + /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: + * the gravity should be canceled from the accelerometer measurements, bust since position + * is added with a delta velocity from a previous term, the actual delta time is more complicated. + * Need to figure out this in the future - currently because of this issue we'll get some more error + * in Z axis. + */ + + /* Rotation term */ + Vector body_deltaAngles_body = delta_angles; + + // Convert earth-related terms into the body frame + Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); + Vector body_rho = body_R_world * world_rho; + Vector body_omega_earth = body_R_world * world_omega_earth; + + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; + + // debug + // std::cout<<"Pose1.rotation(): "<bias_acc(); + delta_BiasGyro -= Bias_initial_->bias_gyro(); + } + + Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); + Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3); + + Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; + + return predictVelocity_inertial(Pose1, Vel1, + delta_vel_in_t0_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } + + static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_vel_in_t0, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { + + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; + + Vector body_deltaVel_body = delta_vel_in_t0; + Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; + + VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); + + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; + + // debug + // std::cout<<"... inside predictVelocity_inertial"< H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const { + + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + *H1 = stack(2, &H1_Pose, &H1_Vel); + } + + // Jacobian w.r.t. Vel1 + if (H2){ + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + *H2 = stack(2, &H2_Pose, &H2_Vel); + } + + // Jacobian w.r.t. IMUBias1 + if (H3){ + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + *H3 = stack(2, &H3_Pose, &H3_Vel); + } + + // Jacobian w.r.t. Pose2 + if (H4){ + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + *H4 = stack(2, &H4_Pose, &H4_Vel); + } + + // Jacobian w.r.t. Vel2 + if (H5){ + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + *H5 = stack(2, &H5_Pose, &H5_Vel); + } + + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } + + + + static inline POSE PredictPoseFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, + const boost::optional& Bias_initial = boost::none) { + + + // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.bias_acc(); + Vector delta_BiasGyro = Bias1.bias_gyro(); + if (Bias_initial){ + delta_BiasAcc -= Bias_initial->bias_acc(); + delta_BiasGyro -= Bias_initial->bias_gyro(); + } + + Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(4,9,3,3); + Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(4,12,3,3); + Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(0,12,3,3); + + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0 + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; + + /* Rotation term */ + Vector delta_angles_corrected = delta_angles + J_angles_wrt_BiasGyro*delta_BiasGyro; + // Another alternative: + // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); + + return predictPose_inertial(Pose1, Vel1, delta_pos_in_t0_corrected, delta_angles_corrected, dt12, world_g, world_rho, world_omega_earth); + } + + static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, + const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, + const boost::optional& Bias_initial = boost::none) { + + // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.bias_acc(); + Vector delta_BiasGyro = Bias1.bias_gyro(); + if (Bias_initial){ + delta_BiasAcc -= Bias_initial->bias_acc(); + delta_BiasGyro -= Bias_initial->bias_gyro(); + } + + Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(6,9,3,3); + Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(6,12,3,3); + + Vector delta_vel_in_t0_corrected = delta_vel_in_t0 + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; + + return predictVelocity_inertial(Pose1, Vel1, delta_vel_in_t0_corrected, dt12, world_g, world_rho, world_omega_earth); + } + + static inline void PredictFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, + const boost::optional& Bias_initial = boost::none) { + + Pose2 = PredictPoseFromPreIntegration(Pose1, Vel1, Bias1, delta_pos_in_t0, delta_angles, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial); + Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial); + } + + + + + + + static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt, + Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, + const noiseModel::Gaussian::shared_ptr& model_continuous_overall, + Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), + boost::optional p_body_P_sensor = boost::none){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: Earth-related terms are not accounted here but are incorporate in predict functions. + + POSE body_P_sensor = POSE(); + bool flag_use_body_P_sensor = false; + if (p_body_P_sensor){ + body_P_sensor = *p_body_P_sensor; + flag_use_body_P_sensor = true; + } + + delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); + delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + + delta_t += msr_dt; + + // Update EquivCov_Overall + Matrix Z_3x3 = zeros(3,3); + Matrix I_3x3 = eye(3,3); + + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_angles = Z_3x3; + Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); + + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_pos = Z_3x3; + + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; + + Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias); + Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias); + Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias); + Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3); + Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); + Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); + + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); + Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + + EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + + // Update Jacobian_wrt_t0_Overall + Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; + } + + static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + + return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; + } + + + + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector AccCorrected = Bias_t0.CorrectAcc(msr_acc_t); + Vector body_t_a_body; + if (flag_use_body_P_sensor){ + Matrix body_R_sensor = body_P_sensor.rotation().matrix(); + + Vector GyroCorrected(Bias_t0.CorrectGyro(msr_gyro_t)); + + Vector body_omega_body = body_R_sensor * GyroCorrected; + Matrix body_omega_body__cross = skewSymmetric(body_omega_body); + + body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); + } else{ + body_t_a_body = AccCorrected; + } + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector GyroCorrected = Bias_t0.CorrectGyro(msr_gyro_t); + + Vector body_t_omega_body; + if (flag_use_body_P_sensor){ + body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; + } else { + body_t_omega_body = GyroCorrected; + } + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + + + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ + + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; + + return noiseModel::Gaussian::Covariance(cov_process); + } + + static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process, + Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){ + + cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + } + + static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, + Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { + + Matrix ENU_to_NED = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); + + Matrix NED_to_ENU = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); + + // Convert incoming parameters to ENU + Vector Pos_ENU = NED_to_ENU * Pos_NED; + Vector Vel_ENU = NED_to_ENU * Vel_NED; + Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; + + // Call ENU version + Vector g_ENU; + Vector rho_ENU; + Vector omega_earth_ENU; + Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); + + // Convert output to NED + g_NED = ENU_to_NED * g_ENU; + rho_NED = ENU_to_NED * rho_ENU; + omega_earth_NED = ENU_to_NED * omega_earth_ENU; + } + + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); + + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); + + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); + + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); + + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + + Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + + + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + } + + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); + } +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } + + + +}; // \class EquivInertialNavFactor_GlobalVel + +} /// namespace gtsam diff --git a/gtsam_unstable/navigation/ImuBias.h b/gtsam_unstable/navigation/ImuBias.h new file mode 100644 index 000000000..b9cfebf46 --- /dev/null +++ b/gtsam_unstable/navigation/ImuBias.h @@ -0,0 +1,182 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ImuBias.h + * @date Feb 2, 2012 + * @author Vadim Indelman, Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include + +/* + * NOTES: + * - Earth-rate correction: + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defened by the user). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. + */ + +namespace gtsam { + +/// All bias models live in the imuBias namespace +namespace imuBias { + + class ConstantBias : public DerivedValue { + private: + Vector bias_acc_; + Vector bias_gyro_; + + public: + + ConstantBias(): + bias_acc_(Vector_(3, 0.0, 0.0, 0.0)), bias_gyro_(Vector_(3, 0.0, 0.0, 0.0)) { + } + + ConstantBias(const Vector& bias_acc, const Vector& bias_gyro): + bias_acc_(bias_acc), bias_gyro_(bias_gyro) { + } + + Vector CorrectAcc(Vector measurment, boost::optional H=boost::none) const { + if (H){ + Matrix zeros3_3(zeros(3,3)); + Matrix m_eye3(-eye(3)); + + *H = collect(2, &m_eye3, &zeros3_3); + } + + return measurment - bias_acc_; + } + + + Vector CorrectGyro(Vector measurment, boost::optional H=boost::none) const { + if (H){ + Matrix zeros3_3(zeros(3,3)); + Matrix m_eye3(-eye(3)); + + *H = collect(2, &zeros3_3, &m_eye3); + } + + return measurment - bias_gyro_; + } + + // H1: Jacobian w.r.t. IMUBias + // H2: Jacobian w.r.t. pose + Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G, + boost::optional H1=boost::none, boost::optional H2=boost::none) const { + + Matrix R_G_to_I( pose.rotation().matrix().transpose() ); + Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G; + + if (H1){ + Matrix zeros3_3(zeros(3,3)); + Matrix m_eye3(-eye(3)); + + *H1 = collect(2, &zeros3_3, &m_eye3); + } + + if (H2){ + Matrix zeros3_3(zeros(3,3)); + Matrix H = -skewSymmetric(w_earth_rate_I); + + *H2 = collect(2, &H, &zeros3_3); + } + + //TODO: Make sure H2 is correct. + + return measurement - bias_gyro_ - w_earth_rate_I; + +// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; + } + + /** Expmap around identity */ + static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } + + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const ConstantBias& p) { return concatVectors(2, &p.bias_acc_, &p.bias_gyro_); } + + /** Update the LieVector with a tangent space update */ + inline ConstantBias retract(const Vector& v) const { return ConstantBias(bias_acc_ + v.head(3), bias_gyro_ + v.tail(3)); } + + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const ConstantBias& t2) const { + Vector delta_acc(t2.bias_acc_ - bias_acc_); + Vector delta_gyro(t2.bias_gyro_ - bias_gyro_); + return concatVectors(2, &delta_acc, &delta_gyro); + } + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->bias_acc_.size() + this->bias_gyro_.size(); } + + /// print with optional string + void print(const std::string& s = "") const { + // explicit printing for now. + std::cout << s + ".bias_acc [" << bias_acc_.transpose() << "]" << std::endl; + std::cout << s + ".bias_gyro [" << bias_gyro_.transpose() << "]" << std::endl; + } + + /** equality up to tolerance */ + inline bool equals(const ConstantBias& expected, double tol=1e-5) const { + return equal(bias_acc_, expected.bias_acc_, tol) && equal(bias_gyro_, expected.bias_gyro_, tol); + } + + /** get bias_acc */ + const Vector& bias_acc() const { return bias_acc_; } + + /** get bias_gyro */ + const Vector& bias_gyro() const { return bias_gyro_; } + + + ConstantBias compose(const ConstantBias& b2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(b2.dim()); + + return ConstantBias(bias_acc_ + b2.bias_acc_, bias_gyro_ + b2.bias_gyro_); + } + + /** between operation */ + ConstantBias between(const ConstantBias& b2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(b2.dim()); + return ConstantBias(b2.bias_acc_ - bias_acc_, b2.bias_gyro_ - bias_gyro_); + } + + /** invert the object and yield a new one */ + inline ConstantBias inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); + + return ConstantBias(-1.0 * bias_acc_, -1.0 * bias_gyro_); + } + + + + }; // ConstantBias class + + +} // namespace ImuBias + +} // namespace gtsam + + diff --git a/gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h new file mode 100644 index 000000000..280b001af --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testEquivInertialNavFactor_GlobalVel.h.cpp + * @brief Unit test for the InertialNavFactor_GlobalVelocity + * @author Vadim Indelman, Stephen Williams + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( EquivInertialNavFactor_GlobalVel, Constructor) +{ + Key poseKey1(11); + Key poseKey2(12); + Key velKey1(21); + Key velKey2(22); + Key biasKey1(31); + + // IMU accumulation variables + Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0); + Vector delta_vel_in_t0 = Vector_(3, 0.0, 0.0, 0.0); + Vector delta_angles = Vector_(3, 0.0, 0.0, 0.0); + double delta_t = 0.0; + Matrix EquivCov_Overall = zeros(15,15); + Matrix Jacobian_wrt_t0_Overall = eye(15); + imuBias::ConstantBias bias1 = imuBias::ConstantBias(); + + // Earth Terms (gravity, etc) + Vector3 g(0.0, 0.0, -9.80); + Vector3 rho(0.0, 0.0, 0.0); + Vector3 omega_earth(0.0, 0.0, 0.0); + + // IMU Noise Model + SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); + + // Constructor + EquivInertialNavFactor_GlobalVel( + poseKey1, velKey1, biasKey1, poseKey2, velKey2, + delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t, + g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1)); + +} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/navigation/tests/testImuBias.cpp b/gtsam_unstable/navigation/tests/testImuBias.cpp new file mode 100644 index 000000000..4f36f4999 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testImuBias.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testInertialNavFactor.cpp + * @brief Unit test for the InertialNavFactor + * @author Vadim Indelman, Stephen Williams + */ + +#include +#include + +using namespace std; +using namespace gtsam; + + +/* ************************************************************************* */ +TEST( ImuBias, Constructor) +{ + Vector bias_acc(Vector_(3,0.1,0.2,0.4)); + Vector bias_gyro(Vector_(3, -0.2, 0.5, 0.03)); + + // Default Constructor + gtsam::imuBias::ConstantBias bias1; + + // Acc + Gyro Constructor + gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); + + // Copy Constructor + gtsam::imuBias::ConstantBias bias3(bias2); +} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */