gtsam  4.1.0
gtsam
EquivInertialNavFactor_GlobalVel_NoBias.h
Go to the documentation of this file.
1 
2 /* ----------------------------------------------------------------------------
3 
4  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
5  * Atlanta, Georgia 30332-0415
6  * All Rights Reserved
7  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
8 
9  * See LICENSE for the license information
10 
11  * -------------------------------------------------------------------------- */
12 
20 #pragma once
21 
24 #include <gtsam/geometry/Rot3.h>
25 #include <gtsam/base/Matrix.h>
26 
27 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
29 
30 #include <boost/optional.hpp>
31 
32 #include <ostream>
33 
34 namespace gtsam {
35 
36 /*
37  * NOTES:
38  * =====
39  * Concept: Based on [Lupton12tro]
40  * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
41  * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
42  * All sensor-to-body transformations are performed here.
43  * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
44  * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will
45  * relate between navigation variables at the two time instances (t0 and current time).
46  *
47  * Other notes:
48  * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame.
49  * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames.
50  * - Camera and IMU frames are identical
51  * - The user should specify a continuous equivalent noise covariance, which can be calculated using
52  * the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance
53  * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
54  * discrete form using the supplied delta_t between sub-sequential measurements.
55  * - Earth-rate correction:
56  * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
57  * frame (Local-Level system: ENU or NED, see above).
58  * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
59  * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
60  * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
61  *
62  * - Frame Notation:
63  * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
64  * So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor
65  * And the transformation from the body frame to the world frame would be: world_P_body
66  * This allows visual chaining. For example, converting the sensed angular velocity of the IMU
67  * (angular velocity of the sensor in the sensor frame) into the world frame can be performed as:
68  * world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor
69  *
70  *
71  * - Common Quantity Types
72  * P : pose/3d transformation
73  * R : rotation
74  * omega : angular velocity
75  * t : translation
76  * v : velocity
77  * a : acceleration
78  *
79  * - Common Frames
80  * sensor : the coordinate system attached to the sensor origin
81  * body : the coordinate system attached to body/inertial frame.
82  * Unless an optional frame transformation is provided, the
83  * sensor frame and the body frame will be identical
84  * world : the global/world coordinate frame. This is assumed to be
85  * a tangent plane to the earth's surface somewhere near the
86  * vehicle
87  */
88 
89 template<class POSE, class VELOCITY>
90 class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> {
91 
92 private:
93 
96 
97  Vector delta_pos_in_t0_;
98  Vector delta_vel_in_t0_;
99  Vector3 delta_angles_;
100  double dt12_;
101 
102  Vector world_g_;
103  Vector world_rho_;
104  Vector world_omega_earth_;
105 
106  Matrix Jacobian_wrt_t0_Overall_;
107 
108  boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
109 
110 public:
111 
112  // shorthand for a smart pointer to a factor
113  typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
114 
117 
119  EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2,
120  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
121  double dt12, const Vector world_g, const Vector world_rho,
122  const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
123  const Matrix& Jacobian_wrt_t0_Overall,
124  boost::optional<POSE> body_P_sensor = boost::none) :
125  Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
126  delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
127  dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
128  body_P_sensor_(body_P_sensor) { }
129 
131 
135  virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
136  std::cout << s << "("
137  << keyFormatter(this->key1()) << ","
138  << keyFormatter(this->key2()) << ","
139  << keyFormatter(this->key3()) << ","
140  << keyFormatter(this->key4()) << "\n";
141  std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
142  std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
143  std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
144  std::cout << "dt12: " << this->dt12_ << std::endl;
145  std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
146  std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
147  std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
148  if(this->body_P_sensor_)
149  this->body_P_sensor_->print(" sensor pose in body frame: ");
150  this->noiseModel_->print(" noise model");
151  }
152 
154  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
155  const This *e = dynamic_cast<const This*> (&expected);
156  return e != nullptr && Base::equals(*e, tol)
157  && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
158  && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
159  && (delta_angles_ - e->delta_angles_).norm() < tol
160  && (dt12_ - e->dt12_) < tol
161  && (world_g_ - e->world_g_).norm() < tol
162  && (world_rho_ - e->world_rho_).norm() < tol
163  && (world_omega_earth_ - e->world_omega_earth_).norm() < tol
164  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
165  }
166 
167 
168  POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const {
169 
170  /* Position term */
171  Vector delta_pos_in_t0_corrected = delta_pos_in_t0_;
172 
173  /* Rotation term */
174  Vector delta_angles_corrected = delta_angles_;
175 
176  return predictPose_inertial(Pose1, Vel1,
177  delta_pos_in_t0_corrected, delta_angles_corrected,
178  dt12_, world_g_, world_rho_, world_omega_earth_);
179  }
180 
181  static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
182  const Vector& delta_pos_in_t0, const Vector3& delta_angles,
183  const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
184 
185  const POSE& world_P1_body = Pose1;
186  const VELOCITY& world_V1_body = Vel1;
187 
188  /* Position term */
189  Vector body_deltaPos_body = delta_pos_in_t0;
190 
191  Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
192  Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
193 
194  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
195  world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
196 
197  /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
198  * the gravity should be canceled from the accelerometer measurements, bust since position
199  * is added with a delta velocity from a previous term, the actual delta time is more complicated.
200  * Need to figure out this in the future - currently because of this issue we'll get some more error
201  * in Z axis.
202  */
203 
204  /* Rotation term */
205  Vector body_deltaAngles_body = delta_angles;
206 
207  // Convert earth-related terms into the body frame
208  Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
209  Vector body_rho = body_R_world * world_rho;
210  Vector body_omega_earth = body_R_world * world_omega_earth;
211 
212  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
213  body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
214 
215  return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
216 
217  }
218 
219  VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const {
220 
221 
222  Vector delta_vel_in_t0_corrected = delta_vel_in_t0_;
223 
224  return predictVelocity_inertial(Pose1, Vel1,
225  delta_vel_in_t0_corrected,
226  dt12_, world_g_, world_rho_, world_omega_earth_);
227  }
228 
229  static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
230  const Vector& delta_vel_in_t0,
231  const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
232 
233  const POSE& world_P1_body = Pose1;
234  const VELOCITY& world_V1_body = Vel1;
235 
236  Vector body_deltaVel_body = delta_vel_in_t0;
237  Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
238 
239  VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
240 
241  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
242  VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
243 
244  // Predict
245  return Vel1.compose( VelDelta );
246 
247  }
248 
249  void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const {
250  Pose2 = predictPose(Pose1, Vel1);
251  Vel2 = predictVelocity(Pose1, Vel1);
252  }
253 
254  POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
255  // Predict
256  POSE Pose2Pred = predictPose(Pose1, Vel1);
257 
258  // Calculate error
259  return Pose2.between(Pose2Pred);
260  }
261 
262  VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
263  // Predict
264  VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1);
265 
266  // Calculate error
267  return Vel2.between(Vel2Pred);
268  }
269 
270  Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2,
271  boost::optional<Matrix&> H1 = boost::none,
272  boost::optional<Matrix&> H2 = boost::none,
273  boost::optional<Matrix&> H3 = boost::none,
274  boost::optional<Matrix&> H4 = boost::none) const {
275 
276  // TODO: Write analytical derivative calculations
277  // Jacobian w.r.t. Pose1
278  if (H1){
279  Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
280  Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
281  *H1 = stack(2, &H1_Pose, &H1_Vel);
282  }
283 
284  // Jacobian w.r.t. Vel1
285  if (H2){
286  Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
287  Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
288  *H2 = stack(2, &H2_Pose, &H2_Vel);
289  }
290 
291  // Jacobian w.r.t. Pose2
292  if (H3){
293  Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
294  Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
295  *H3 = stack(2, &H3_Pose, &H3_Vel);
296  }
297 
298  // Jacobian w.r.t. Vel2
299  if (H4){
300  Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
301  Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
302  *H4 = stack(2, &H4_Pose, &H4_Vel);
303  }
304 
305  Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2)));
306  Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2)));
307 
308  return concatVectors(2, &ErrPoseVector, &ErrVelVector);
309  }
310 
311 
312 
313  static inline POSE PredictPoseFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1,
314  const Vector& delta_pos_in_t0, const Vector3& delta_angles,
315  double dt12, const Vector world_g, const Vector world_rho,
316  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall) {
317 
318  /* Position term */
319  Vector delta_pos_in_t0_corrected = delta_pos_in_t0;
320 
321  /* Rotation term */
322  Vector delta_angles_corrected = delta_angles;
323  // Another alternative:
324  // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
325 
326  return predictPose_inertial(Pose1, Vel1, delta_pos_in_t0_corrected, delta_angles_corrected, dt12, world_g, world_rho, world_omega_earth);
327  }
328 
329  static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1,
330  const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho,
331  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall) {
332 
333  Vector delta_vel_in_t0_corrected = delta_vel_in_t0;
334 
335  return predictVelocity_inertial(Pose1, Vel1, delta_vel_in_t0_corrected, dt12, world_g, world_rho, world_omega_earth);
336  }
337 
338  static inline void PredictFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2,
339  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
340  double dt12, const Vector world_g, const Vector world_rho,
341  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall) {
342 
343  Pose2 = PredictPoseFromPreIntegration(Pose1, Vel1, delta_pos_in_t0, delta_angles, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall);
344  Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall);
345  }
346 
347 
348  static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
349  Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
350  const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
351  Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
352  boost::optional<POSE> p_body_P_sensor = boost::none){
353  // Note: all delta terms refer to an IMU\sensor system at t0
354  // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
355 
356  POSE body_P_sensor = POSE();
357  bool flag_use_body_P_sensor = false;
358  if (p_body_P_sensor){
359  body_P_sensor = *p_body_P_sensor;
360  flag_use_body_P_sensor = true;
361  }
362 
363  delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
364  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);
365  delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor);
366 
367  delta_t += msr_dt;
368 
369  // Update EquivCov_Overall
370  Matrix Z_3x3 = Z_3x3;
371  Matrix I_3x3 = I_3x3;
372 
373  Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
374  Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
375  Matrix H_pos_angles = Z_3x3;
376 
377  Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
378  Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(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), delta_angles);
379  Matrix H_vel_pos = Z_3x3;
380 
381  Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
382  Matrix H_angles_pos = Z_3x3;
383  Matrix H_angles_vel = Z_3x3;
384 
385  Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel);
386  Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel);
387  Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel);
388  Matrix F = stack(3, &F_angles, &F_pos, &F_vel);
389 
390  noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
391  Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
392 
393  EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
394 
395  // Update Jacobian_wrt_t0_Overall
396  Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
397  }
398 
399  static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
400  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
401 
402  // Note: all delta terms refer to an IMU\sensor system at t0
403  // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
404 
405  return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
406  }
407 
408 
409 
410  static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
411  const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
412 
413  // Note: all delta terms refer to an IMU\sensor system at t0
414 
415  // Calculate the corrected measurements using the Bias object
416  Vector AccCorrected = msr_acc_t;
417  Vector body_t_a_body;
418  if (flag_use_body_P_sensor){
419  Matrix body_R_sensor = body_P_sensor.rotation().matrix();
420 
421  Vector GyroCorrected(msr_gyro_t);
422 
423  Vector body_omega_body = body_R_sensor * GyroCorrected;
424  Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
425 
426  body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
427  } else{
428  body_t_a_body = AccCorrected;
429  }
430 
431  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
432 
433  return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
434  }
435 
436 
437  static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
438  const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
439 
440  // Note: all delta terms refer to an IMU\sensor system at t0
441 
442  // Calculate the corrected measurements using the Bias object
443  Vector GyroCorrected = msr_gyro_t;
444 
445  Vector body_t_omega_body;
446  if (flag_use_body_P_sensor){
447  body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
448  } else {
449  body_t_omega_body = GyroCorrected;
450  }
451 
452  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
453 
454  R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
455  return Rot3::Logmap(R_t_to_t0);
456  }
457 
458  static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
459  const noiseModel::Gaussian::shared_ptr& gaussian_process){
460 
461  Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
462  Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
463  Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
464 
465  cov_process.block(0,0, 3,3) += cov_gyro;
466  cov_process.block(6,6, 3,3) += cov_acc;
467 
468  return noiseModel::Gaussian::Covariance(cov_process);
469  }
470 
471  static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
472  const noiseModel::Gaussian::shared_ptr& gaussian_process,
473  Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){
474 
475  cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
476  cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
477  cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
478  }
479 
480  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,
481  Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
482 
483  Matrix ENU_to_NED = (Matrix(3, 3) <<
484  0.0, 1.0, 0.0,
485  1.0, 0.0, 0.0,
486  0.0, 0.0, -1.0).finished();
487 
488  Matrix NED_to_ENU = (Matrix(3, 3) <<
489  0.0, 1.0, 0.0,
490  1.0, 0.0, 0.0,
491  0.0, 0.0, -1.0).finished();
492 
493  // Convert incoming parameters to ENU
494  Vector Pos_ENU = NED_to_ENU * Pos_NED;
495  Vector Vel_ENU = NED_to_ENU * Vel_NED;
496  Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
497 
498  // Call ENU version
499  Vector g_ENU;
500  Vector rho_ENU;
501  Vector omega_earth_ENU;
502  Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
503 
504  // Convert output to NED
505  g_NED = ENU_to_NED * g_ENU;
506  rho_NED = ENU_to_NED * rho_ENU;
507  omega_earth_NED = ENU_to_NED * omega_earth_ENU;
508  }
509 
510  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,
511  Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
512  double R0 = 6.378388e6;
513  double e = 1/297;
514  double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
515 
516  // Calculate current lat, lon
517  Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
518  double delta_lat(delta_Pos_ENU(1)/Re);
519  double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
520  double lat_new(LatLonHeight_IC(0) + delta_lat);
521  double lon_new(LatLonHeight_IC(1) + delta_lon);
522 
523  // Rotation of lon about z axis
524  Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
525  -sin(lon_new), cos(lon_new), 0.0,
526  0.0, 0.0, 1.0);
527 
528  // Rotation of lat about y axis
529  Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
530  0.0, 1.0, 0.0,
531  -sin(lat_new), 0.0, cos(lat_new));
532 
533  Rot3 UEN_to_ENU(0, 1, 0,
534  0, 0, 1,
535  1, 0, 0);
536 
537  Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
538 
539  Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
540  omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
541 
542  // Calculating g
543  double height(LatLonHeight_IC(2));
544  double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
545  double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
546  double e2( pow(ECCENTRICITY,2) );
547  double den( 1-e2*pow(sin(lat_new),2) );
548  double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
549  double Rp( EQUA_RADIUS/( sqrt(den) ) );
550  double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
551  double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
552  double g_calc( g0/( pow(1 + height/Ro, 2) ) );
553  g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
554 
555 
556  // Calculate rho
557  double Ve( Vel_ENU(0) );
558  double Vn( Vel_ENU(1) );
559  double rho_E = -Vn/(Rm + height);
560  double rho_N = Ve/(Rp + height);
561  double rho_U = Ve*tan(lat_new)/(Rp + height);
562  rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
563  }
564 
565  static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
566  /* Q_d (approx)= Q * delta_t */
567  /* In practice, square root of the information matrix is represented, so that:
568  * R_d (approx)= R / sqrt(delta_t)
569  * */
570  return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
571  }
572 private:
573 
576  template<class ARCHIVE>
577  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
578  ar & boost::serialization::make_nvp("NonlinearFactor2",
579  boost::serialization::base_object<Base>(*this));
580  }
581 
582 
583 
584 }; // \class EquivInertialNavFactor_GlobalVel_NoBias
585 
586 }
gtsam::EquivInertialNavFactor_GlobalVel_NoBias
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:90
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:154
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Pose2
Definition: Pose2.h:36
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::evaluateError
Vector evaluateError(const POSE &Pose1, const VELOCITY &Vel1, const POSE &Pose2, const VELOCITY &Vel2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const
Override this method to finish implementing a 4-way factor.
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:270
gtsam::collect
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
create a matrix by concatenating Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:442
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::EquivInertialNavFactor_GlobalVel_NoBias
EquivInertialNavFactor_GlobalVel_NoBias(const Key &Pose1, const Key &Vel1, 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< POSE > body_P_sensor=boost::none)
Constructor.
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:119
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::access
friend class boost::serialization::access
Serialization function.
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:575
NonlinearFactor.h
Non-linear factor base classes.
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:116
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::EquivInertialNavFactor_GlobalVel_NoBias
EquivInertialNavFactor_GlobalVel_NoBias()
default constructor - only use for serialization
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:116
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition: Vector.cpp:302
gtsam::NoiseModelFactor4< POSE, VELOCITY, POSE, VELOCITY >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:543
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
Definition: Matrix.h:404
gtsam::Rot3::Logmap
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition: Rot3M.cpp:158
gtsam::testing::inverse
T inverse(const T &t)
unary functions
Definition: lieProxies.h:43
gtsam::NoiseModelFactor4
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:507
gtsam::noiseModel::Gaussian::SqrtInformation
static shared_ptr SqrtInformation(const Matrix &R, bool smart=true)
A Gaussian noise model created by specifying a square root information matrix.
Definition: NoiseModel.cpp:85
NoiseModel.h
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates using Rodrigues' formula.
Definition: Rot3.h:377
gtsam::EquivInertialNavFactor_GlobalVel_NoBias::print
virtual void print(const std::string &s="EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
implement functions needed for Testable
Definition: EquivInertialNavFactor_GlobalVel_NoBias.h:135
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:396