EquivInertialNavFactor_GlobalVel.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 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, class IMUBIAS>
90 class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
91 
92 private:
93 
96 
100  double dt12_;
101 
105 
107 
108  boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
109  boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
110 
111 public:
112 
113  // shorthand for a smart pointer to a factor
114  typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
115 
118 
120  EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
121  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
122  double dt12, const Vector world_g, const Vector world_rho,
123  const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
124  const Matrix& Jacobian_wrt_t0_Overall,
125  boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
126  Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
127  delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
128  dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
129  Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
130 
132 
136  void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
137  std::cout << s << "("
138  << keyFormatter(this->key1()) << ","
139  << keyFormatter(this->key2()) << ","
140  << keyFormatter(this->key3()) << ","
141  << keyFormatter(this->key4()) << ","
142  << keyFormatter(this->key5()) << "\n";
143  std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
144  std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
145  std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
146  std::cout << "dt12: " << this->dt12_ << std::endl;
147  std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
148  std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
149  std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
150  if(this->body_P_sensor_)
151  this->body_P_sensor_->print(" sensor pose in body frame: ");
152  this->noiseModel_->print(" noise model");
153  }
154 
156  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
157  const This *e = dynamic_cast<const This*> (&expected);
158  return e != nullptr && Base::equals(*e, tol)
159  && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
160  && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
161  && (delta_angles_ - e->delta_angles_).norm() < tol
162  && (dt12_ - e->dt12_) < tol
163  && (world_g_ - e->world_g_).norm() < tol
164  && (world_rho_ - e->world_rho_).norm() < tol
165  && (world_omega_earth_ - e->world_omega_earth_).norm() < tol
166  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
167  }
168 
169 
170  POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
171 
172  // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
173  Vector delta_BiasAcc = Bias1.accelerometer();
174  Vector delta_BiasGyro = Bias1.gyroscope();
175  if (Bias_initial_){
176  delta_BiasAcc -= Bias_initial_->accelerometer();
177  delta_BiasGyro -= Bias_initial_->gyroscope();
178  }
179 
180  Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
181  Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3);
182  Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3);
183 
184  /* Position term */
185  Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
186 
187  /* Rotation term */
188  Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
189  // Another alternative:
190  // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
191 
192  return predictPose_inertial(Pose1, Vel1,
193  delta_pos_in_t0_corrected, delta_angles_corrected,
194  dt12_, world_g_, world_rho_, world_omega_earth_);
195  }
196 
197  static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
198  const Vector& delta_pos_in_t0, const Vector3& delta_angles,
199  const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
200 
201  const POSE& world_P1_body = Pose1;
202  const VELOCITY& world_V1_body = Vel1;
203 
204  /* Position term */
205  Vector body_deltaPos_body = delta_pos_in_t0;
206 
207  Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
208  Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
209 
210  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
211  world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
212 
213  /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
214  * the gravity should be canceled from the accelerometer measurements, bust since position
215  * is added with a delta velocity from a previous term, the actual delta time is more complicated.
216  * Need to figure out this in the future - currently because of this issue we'll get some more error
217  * in Z axis.
218  */
219 
220  /* Rotation term */
221  Vector body_deltaAngles_body = delta_angles;
222 
223  // Convert earth-related terms into the body frame
224  Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
225  Vector body_rho = body_R_world * world_rho;
226  Vector body_omega_earth = body_R_world * world_omega_earth;
227 
228  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
229  body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
230 
231  return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
232 
233  }
234 
235  VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
236 
237  // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
238  Vector delta_BiasAcc = Bias1.accelerometer();
239  Vector delta_BiasGyro = Bias1.gyroscope();
240  if (Bias_initial_){
241  delta_BiasAcc -= Bias_initial_->accelerometer();
242  delta_BiasGyro -= Bias_initial_->gyroscope();
243  }
244 
245  Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
246  Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3);
247 
248  Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
249 
250  return predictVelocity_inertial(Pose1, Vel1,
251  delta_vel_in_t0_corrected,
252  dt12_, world_g_, world_rho_, world_omega_earth_);
253  }
254 
255  static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
256  const Vector& delta_vel_in_t0,
257  const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
258 
259  const POSE& world_P1_body = Pose1;
260  const VELOCITY& world_V1_body = Vel1;
261 
262  Vector body_deltaVel_body = delta_vel_in_t0;
263  Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
264 
265  VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
266 
267  // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
268  VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
269 
270  // Predict
271  return Vel1 + VelDelta;
272 
273  }
274 
275  void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
276  Pose2 = predictPose(Pose1, Vel1, Bias1);
277  Vel2 = predictVelocity(Pose1, Vel1, Bias1);
278  }
279 
280  POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
281  // Predict
282  POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
283 
284  // Luca: difference between Pose2 and Pose2Pred
285  POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
286 // DiffPose = Pose2.between(Pose2Pred);
287  return DiffPose;
288  // Calculate error
289  //return Pose2.between(Pose2Pred);
290  }
291 
292  VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
293  // Predict
294  VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
295 
296  // Calculate error
297  return Vel2Pred-Vel2;
298  }
299 
300  Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
301  boost::optional<Matrix&> H1 = boost::none,
302  boost::optional<Matrix&> H2 = boost::none,
303  boost::optional<Matrix&> H3 = boost::none,
304  boost::optional<Matrix&> H4 = boost::none,
305  boost::optional<Matrix&> H5 = boost::none) const override {
306 
307  // TODO: Write analytical derivative calculations
308  // Jacobian w.r.t. Pose1
309  if (H1){
310  Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
311  Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
312  *H1 = stack(2, &H1_Pose, &H1_Vel);
313  }
314 
315  // Jacobian w.r.t. Vel1
316  if (H2){
317  if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
318  Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
319  Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
320  *H2 = stack(2, &H2_Pose, &H2_Vel);
321  }
322 
323  // Jacobian w.r.t. IMUBias1
324  if (H3){
325  Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
326  Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
327  *H3 = stack(2, &H3_Pose, &H3_Vel);
328  }
329 
330  // Jacobian w.r.t. Pose2
331  if (H4){
332  Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
333  Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
334  *H4 = stack(2, &H4_Pose, &H4_Vel);
335  }
336 
337  // Jacobian w.r.t. Vel2
338  if (H5){
339  if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
340  Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
341  Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
342  *H5 = stack(2, &H5_Pose, &H5_Vel);
343  }
344 
345  Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
346  Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
347 
348  return concatVectors(2, &ErrPoseVector, &ErrVelVector);
349  }
350 
351 
352 
353  static inline POSE PredictPoseFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
354  const Vector& delta_pos_in_t0, const Vector3& delta_angles,
355  double dt12, const Vector world_g, const Vector world_rho,
356  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
357  const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
358 
359 
360  // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
361  Vector delta_BiasAcc = Bias1.accelerometer();
362  Vector delta_BiasGyro = Bias1.gyroscope();
363  if (Bias_initial){
364  delta_BiasAcc -= Bias_initial->accelerometer();
365  delta_BiasGyro -= Bias_initial->gyroscope();
366  }
367 
368  Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(4,9,3,3);
369  Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(4,12,3,3);
370  Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(0,12,3,3);
371 
372  /* Position term */
373  Vector delta_pos_in_t0_corrected = delta_pos_in_t0 + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
374 
375  /* Rotation term */
376  Vector delta_angles_corrected = delta_angles + J_angles_wrt_BiasGyro*delta_BiasGyro;
377  // Another alternative:
378  // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
379 
380  return predictPose_inertial(Pose1, Vel1, delta_pos_in_t0_corrected, delta_angles_corrected, dt12, world_g, world_rho, world_omega_earth);
381  }
382 
383  static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
384  const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho,
385  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
386  const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
387 
388  // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
389  Vector delta_BiasAcc = Bias1.accelerometer();
390  Vector delta_BiasGyro = Bias1.gyroscope();
391  if (Bias_initial){
392  delta_BiasAcc -= Bias_initial->accelerometer();
393  delta_BiasGyro -= Bias_initial->gyroscope();
394  }
395 
396  Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(6,9,3,3);
397  Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(6,12,3,3);
398 
399  Vector delta_vel_in_t0_corrected = delta_vel_in_t0 + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
400 
401  return predictVelocity_inertial(Pose1, Vel1, delta_vel_in_t0_corrected, dt12, world_g, world_rho, world_omega_earth);
402  }
403 
404  static inline void PredictFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2,
405  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
406  double dt12, const Vector world_g, const Vector world_rho,
407  const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
408  const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
409 
410  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);
411  Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
412  }
413 
414 
415  static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
416  Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
417  const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
418  Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
419  boost::optional<POSE> p_body_P_sensor = boost::none){
420  // Note: all delta terms refer to an IMU\sensor system at t0
421  // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
422 
423  POSE body_P_sensor = POSE();
424  bool flag_use_body_P_sensor = false;
425  if (p_body_P_sensor){
426  body_P_sensor = *p_body_P_sensor;
427  flag_use_body_P_sensor = true;
428  }
429 
430  delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
431  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);
432  delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
433 
434  delta_t += msr_dt;
435 
436  // Update EquivCov_Overall
437  Matrix Z_3x3 = Z_3x3;
438  Matrix I_3x3 = I_3x3;
439 
440  Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
441  Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
442  Matrix H_pos_angles = Z_3x3;
443  Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
444 
445  Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(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);
446  Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(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);
447  Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(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);
448  Matrix H_vel_pos = Z_3x3;
449 
450  Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
451  Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
452  Matrix H_angles_pos = Z_3x3;
453  Matrix H_angles_vel = Z_3x3;
454 
455  Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
456  Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
457  Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
458  Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
459  Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
460  Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
461 
462 
463  noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
464  Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse();
465 
466  EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
467  // Luca: force identity covariance matrix (for testing purposes)
468  // EquivCov_Overall = Matrix::Identity(15,15);
469 
470  // Update Jacobian_wrt_t0_Overall
471  Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
472  }
473 
474  static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
475  const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
476 
477  // Note: all delta terms refer to an IMU\sensor system at t0
478  // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
479 
480  return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
481  }
482 
483 
484 
485  static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
486  const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
487  IMUBIAS Bias_t0 = IMUBIAS()){
488 
489  // Note: all delta terms refer to an IMU\sensor system at t0
490 
491  // Calculate the corrected measurements using the Bias object
492  Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
493  Vector body_t_a_body;
494  if (flag_use_body_P_sensor){
495  Matrix body_R_sensor = body_P_sensor.rotation().matrix();
496 
497  Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
498 
499  Vector body_omega_body = body_R_sensor * GyroCorrected;
500  Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
501 
502  body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
503  } else{
504  body_t_a_body = AccCorrected;
505  }
506 
507  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
508 
509  return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
510  }
511 
512 
513  static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
514  const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
515  IMUBIAS Bias_t0 = IMUBIAS()){
516 
517  // Note: all delta terms refer to an IMU\sensor system at t0
518 
519  // Calculate the corrected measurements using the Bias object
520  Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
521 
522  Vector body_t_omega_body;
523  if (flag_use_body_P_sensor){
524  body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
525  } else {
526  body_t_omega_body = GyroCorrected;
527  }
528 
529  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
530 
531  R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
532  return Rot3::Logmap(R_t_to_t0);
533  }
534 
535 
537  const noiseModel::Gaussian::shared_ptr& gaussian_process){
538 
539  Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
540  Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
541  Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
542 
543  cov_process.block(0,0, 3,3) += cov_gyro;
544  cov_process.block(6,6, 3,3) += cov_acc;
545 
546  return noiseModel::Gaussian::Covariance(cov_process);
547  }
548 
550  const noiseModel::Gaussian::shared_ptr& gaussian_process,
551  Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){
552 
553  cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
554  cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
555  cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
556  }
557 
558  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,
559  Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
560 
561  Matrix ENU_to_NED = (Matrix(3, 3) <<
562  0.0, 1.0, 0.0,
563  1.0, 0.0, 0.0,
564  0.0, 0.0, -1.0).finished();
565 
566  Matrix NED_to_ENU = (Matrix(3, 3) <<
567  0.0, 1.0, 0.0,
568  1.0, 0.0, 0.0,
569  0.0, 0.0, -1.0).finished();
570 
571  // Convert incoming parameters to ENU
572  Vector Pos_ENU = NED_to_ENU * Pos_NED;
573  Vector Vel_ENU = NED_to_ENU * Vel_NED;
574  Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
575 
576  // Call ENU version
577  Vector g_ENU;
578  Vector rho_ENU;
579  Vector omega_earth_ENU;
580  Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
581 
582  // Convert output to NED
583  g_NED = ENU_to_NED * g_ENU;
584  rho_NED = ENU_to_NED * rho_ENU;
585  omega_earth_NED = ENU_to_NED * omega_earth_ENU;
586  }
587 
588  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,
589  Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
590  double R0 = 6.378388e6;
591  double e = 1/297;
592  double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
593 
594  // Calculate current lat, lon
595  Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
596  double delta_lat(delta_Pos_ENU(1)/Re);
597  double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
598  double lat_new(LatLonHeight_IC(0) + delta_lat);
599  double lon_new(LatLonHeight_IC(1) + delta_lon);
600 
601  // Rotation of lon about z axis
602  Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
603  -sin(lon_new), cos(lon_new), 0.0,
604  0.0, 0.0, 1.0);
605 
606  // Rotation of lat about y axis
607  Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
608  0.0, 1.0, 0.0,
609  -sin(lat_new), 0.0, cos(lat_new));
610 
611  Rot3 UEN_to_ENU(0, 1, 0,
612  0, 0, 1,
613  1, 0, 0);
614 
615  Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
616 
617  Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5));
618  omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
619 
620  // Calculating g
621  double height(LatLonHeight_IC(2));
622  double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
623  double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
624  double e2( pow(ECCENTRICITY,2) );
625  double den( 1-e2*pow(sin(lat_new),2) );
626  double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
627  double Rp( EQUA_RADIUS/( sqrt(den) ) );
628  double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
629  double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
630  double g_calc( g0/( pow(1 + height/Ro, 2) ) );
631  g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished();
632 
633 
634  // Calculate rho
635  double Ve( Vel_ENU(0) );
636  double Vn( Vel_ENU(1) );
637  double rho_E = -Vn/(Rm + height);
638  double rho_N = Ve/(Rp + height);
639  double rho_U = Ve*tan(lat_new)/(Rp + height);
640  rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished();
641  }
642 
644  /* Q_d (approx)= Q * delta_t */
645  /* In practice, square root of the information matrix is represented, so that:
646  * R_d (approx)= R / sqrt(delta_t)
647  * */
648  return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
649  }
650 private:
651 
654  template<class ARCHIVE>
655  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
656  ar & boost::serialization::make_nvp("NonlinearFactor2",
657  boost::serialization::base_object<Base>(*this));
658  }
659 
660 
661 
662 }; // \class EquivInertialNavFactor_GlobalVel
663 
664 }
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:116
boost::shared_ptr< EquivInertialNavFactor_GlobalVel > shared_ptr
static 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())
Eigen::Vector3d Vector3
Definition: Vector.h:43
NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY > Base
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:974
static 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< IMUBIAS > &Bias_initial=boost::none)
void predict(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, POSE &Pose2, VELOCITY &Vel2) const
VELOCITY evaluateVelocityError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2) const
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
void serialize(ARCHIVE &ar, const unsigned int)
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Pose2_ Expmap(const Vector3_ &xi)
POSE predictPose(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1) const
Some functions to compute numerical derivatives.
Matrix3 matrix() const
Definition: Rot3M.cpp:219
static 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< POSE > p_body_P_sensor=boost::none)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
void print(const std::string &s="EquivInertialNavFactor_GlobalVel", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
EIGEN_DEVICE_FUNC const CosReturnType cos() const
static 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)
Signature::Row F
Definition: Signature.cpp:53
static const Vector3 world_rho(0.0,-1.5724e-05, 0.0)
Eigen::VectorXd Vector
Definition: Vector.h:38
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
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< IMUBIAS > Bias_initial=boost::none, boost::optional< POSE > body_P_sensor=boost::none)
std::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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static 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)
EIGEN_DEVICE_FUNC const TanReturnType tan() const
static Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, const Vector &delta_pos_in_t0, const Vector &delta_vel_in_t0)
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Definition: Rot3.h:377
static 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)
static const Vector3 world_g(0.0, 0.0, 9.81)
static 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)
traits
Definition: chartTesting.h:28
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:404
SharedNoiseModel noiseModel_
static noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr &model, double delta_t)
Non-linear factor base classes.
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H=boost::none)
Definition: Rot3M.cpp:158
static 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 stack(size_t nrMatrices,...)
Definition: Matrix.cpp:396
static 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< IMUBIAS > &Bias_initial=boost::none)
POSE evaluatePoseError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2) const
VELOCITY predictVelocity(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1) const
gtsam::Vector world_omega_earth(world_R_ECEF.matrix()*ECEF_omega_earth)
static 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)
boost::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:189
static 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< IMUBIAS > &Bias_initial=boost::none)
static Rot3 R0
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
Definition: Matrix.cpp:442
EIGEN_DEVICE_FUNC const SinReturnType sin() const
const G double tol
Definition: Group.h:83
Vector evaluateError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, 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, boost::optional< Matrix & > H5=boost::none) const override
static shared_ptr SqrtInformation(const Matrix &R, bool smart=true)
Definition: NoiseModel.cpp:85
static 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())
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
Vector concatVectors(const std::list< Vector > &vs)
Definition: Vector.cpp:302
3D rotation represented as a rotation matrix or quaternion
EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS > This


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:02