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
13/**
14 * @file EquivInertialNavFactor_GlobalVel.h
15 * @author Vadim Indelman, Stephen Williams
16 * @brief Equivalent inertial navigation factor (velocity in the global frame).
17 * @date Sep. 26, 2012
18 **/
19
20#pragma once
21
22#include <gtsam/nonlinear/NonlinearFactor.h>
23#include <gtsam/linear/NoiseModel.h>
24#include <gtsam/geometry/Rot3.h>
25#include <gtsam/base/Matrix.h>
26
27// Using numerical derivative to calculate d(Pose3::Expmap)/dw
28#include <gtsam/base/numericalDerivative.h>
29
30#include <ostream>
31
32namespace gtsam {
33
34/*
35 * NOTES:
36 * =====
37 * Concept: Based on [Lupton12tro]
38 * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
39 * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
40 * All sensor-to-body transformations are performed here.
41 * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
42 * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will
43 * relate between navigation variables at the two time instances (t0 and current time).
44 *
45 * Other notes:
46 * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame.
47 * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames.
48 * - Camera and IMU frames are identical
49 * - The user should specify a continuous equivalent noise covariance, which can be calculated using
50 * the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance
51 * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
52 * discrete form using the supplied delta_t between sub-sequential measurements.
53 * - Earth-rate correction:
54 * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
55 * frame (Local-Level system: ENU or NED, see above).
56 * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
57 * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
58 * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
59 *
60 * - Frame Notation:
61 * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
62 * So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor
63 * And the transformation from the body frame to the world frame would be: world_P_body
64 * This allows visual chaining. For example, converting the sensed angular velocity of the IMU
65 * (angular velocity of the sensor in the sensor frame) into the world frame can be performed as:
66 * world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor
67 *
68 *
69 * - Common Quantity Types
70 * P : pose/3d transformation
71 * R : rotation
72 * omega : angular velocity
73 * t : translation
74 * v : velocity
75 * a : acceleration
76 *
77 * - Common Frames
78 * sensor : the coordinate system attached to the sensor origin
79 * body : the coordinate system attached to body/inertial frame.
80 * Unless an optional frame transformation is provided, the
81 * sensor frame and the body frame will be identical
82 * world : the global/world coordinate frame. This is assumed to be
83 * a tangent plane to the earth's surface somewhere near the
84 * vehicle
85 */
86
87template<class POSE, class VELOCITY, class IMUBIAS>
88class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
89
90private:
91
92 typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
93 typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
94
95 Vector delta_pos_in_t0_;
96 Vector delta_vel_in_t0_;
97 Vector3 delta_angles_;
98 double dt12_;
99
100 Vector world_g_;
101 Vector world_rho_;
102 Vector world_omega_earth_;
103
104 Matrix Jacobian_wrt_t0_Overall_;
105
106 std::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
107 std::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
108
109public:
110
111 // Provide access to the Matrix& version of evaluateError:
112 using Base::evaluateError;
113
114 // shorthand for a smart pointer to a factor
115 typedef typename std::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
116
117 /** default constructor - only use for serialization */
118 EquivInertialNavFactor_GlobalVel() {}
119
120 /** Constructor */
121 EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
122 const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
123 double dt12, const Vector world_g, const Vector world_rho,
124 const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
125 const Matrix& Jacobian_wrt_t0_Overall,
126 std::optional<IMUBIAS> Bias_initial = {}, std::optional<POSE> body_P_sensor = {}) :
127 Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
128 delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
129 dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
130 Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
131
132 ~EquivInertialNavFactor_GlobalVel() override {}
133
134 /** implement functions needed for Testable */
135
136 /** print */
137 void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
138 std::cout << s << "("
139 << keyFormatter(this->key1()) << ","
140 << keyFormatter(this->key2()) << ","
141 << keyFormatter(this->key3()) << ","
142 << keyFormatter(this->key4()) << ","
143 << keyFormatter(this->key5()) << "\n";
144 std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
145 std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
146 std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
147 std::cout << "dt12: " << this->dt12_ << std::endl;
148 std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
149 std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
150 std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
151 if(this->body_P_sensor_)
152 this->body_P_sensor_->print(" sensor pose in body frame: ");
153 this->noiseModel_->print(" noise model");
154 }
155
156 /** equals */
157 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
158 const This *e = dynamic_cast<const This*> (&expected);
159 return e != nullptr && Base::equals(*e, tol)
160 && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
161 && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
162 && (delta_angles_ - e->delta_angles_).norm() < tol
163 && (dt12_ - e->dt12_) < tol
164 && (world_g_ - e->world_g_).norm() < tol
165 && (world_rho_ - e->world_rho_).norm() < tol
166 && (world_omega_earth_ - e->world_omega_earth_).norm() < tol
167 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
168 }
169
170
171 POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
172
173 // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
174 Vector delta_BiasAcc = Bias1.accelerometer();
175 Vector delta_BiasGyro = Bias1.gyroscope();
176 if (Bias_initial_){
177 delta_BiasAcc -= Bias_initial_->accelerometer();
178 delta_BiasGyro -= Bias_initial_->gyroscope();
179 }
180
181 Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(startRow: 4,startCol: 9,blockRows: 3,blockCols: 3);
182 Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(startRow: 4,startCol: 12,blockRows: 3,blockCols: 3);
183 Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(startRow: 0,startCol: 12,blockRows: 3,blockCols: 3);
184
185 /* Position term */
186 Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
187
188 /* Rotation term */
189 Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
190 // Another alternative:
191 // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
192
193 return predictPose_inertial(Pose1, Vel1,
194 delta_pos_in_t0: delta_pos_in_t0_corrected, delta_angles: delta_angles_corrected,
195 dt12: dt12_, world_g: world_g_, world_rho: world_rho_, world_omega_earth: world_omega_earth_);
196 }
197
198 static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
199 const Vector& delta_pos_in_t0, const Vector3& delta_angles,
200 const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
201
202 const POSE& world_P1_body = Pose1;
203 const VELOCITY& world_V1_body = Vel1;
204
205 /* Position term */
206 Vector body_deltaPos_body = delta_pos_in_t0;
207
208 Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
209 Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
210
211 // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
212 world_deltaPos_body -= 2*skewSymmetric(w: world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
213
214 /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
215 * the gravity should be canceled from the accelerometer measurements, bust since position
216 * is added with a delta velocity from a previous term, the actual delta time is more complicated.
217 * Need to figure out this in the future - currently because of this issue we'll get some more error
218 * in Z axis.
219 */
220
221 /* Rotation term */
222 Vector body_deltaAngles_body = delta_angles;
223
224 // Convert earth-related terms into the body frame
225 Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
226 Vector body_rho = body_R_world * world_rho;
227 Vector body_omega_earth = body_R_world * world_omega_earth;
228
229 // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
230 body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
231
232 return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
233
234 }
235
236 VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
237
238 // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
239 Vector delta_BiasAcc = Bias1.accelerometer();
240 Vector delta_BiasGyro = Bias1.gyroscope();
241 if (Bias_initial_){
242 delta_BiasAcc -= Bias_initial_->accelerometer();
243 delta_BiasGyro -= Bias_initial_->gyroscope();
244 }
245
246 Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(startRow: 6,startCol: 9,blockRows: 3,blockCols: 3);
247 Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(startRow: 6,startCol: 12,blockRows: 3,blockCols: 3);
248
249 Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
250
251 return predictVelocity_inertial(Pose1, Vel1,
252 delta_vel_in_t0: delta_vel_in_t0_corrected,
253 dt12: dt12_, world_g: world_g_, world_rho: world_rho_, world_omega_earth: world_omega_earth_);
254 }
255
256 static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
257 const Vector& delta_vel_in_t0,
258 const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
259
260 const POSE& world_P1_body = Pose1;
261 const VELOCITY& world_V1_body = Vel1;
262
263 Vector body_deltaVel_body = delta_vel_in_t0;
264 Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
265
266 VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
267
268 // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
269 VelDelta -= 2*skewSymmetric(w: world_rho + world_omega_earth)*world_V1_body * dt12;
270
271 // Predict
272 return Vel1 + VelDelta;
273
274 }
275
276 void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
277 Pose2 = predictPose(Pose1, Vel1, Bias1);
278 Vel2 = predictVelocity(Pose1, Vel1, Bias1);
279 }
280
281 POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
282 // Predict
283 POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
284
285 // Luca: difference between Pose2 and Pose2Pred
286 POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
287// DiffPose = Pose2.between(Pose2Pred);
288 return DiffPose;
289 // Calculate error
290 //return Pose2.between(Pose2Pred);
291 }
292
293 VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
294 // Predict
295 VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
296
297 // Calculate error
298 return Vel2Pred-Vel2;
299 }
300
301 Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
302 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
303 OptionalMatrixType H5) const override {
304
305 // TODO: Write analytical derivative calculations
306 // Jacobian w.r.t. Pose1
307 if (H1){
308 Matrix H1_Pose = numericalDerivative11<POSE, POSE>(
309 std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
310 this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
311 Pose1);
312 Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(
313 std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
314 this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
315 Pose1);
316 *H1 = stack(nrMatrices: 2, &H1_Pose, &H1_Vel);
317 }
318
319 // Jacobian w.r.t. Vel1
320 if (H2){
321 if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
322 Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(
323 std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
324 this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
325 Vel1);
326 Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(
327 std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
328 this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
329 Vel1);
330 *H2 = stack(nrMatrices: 2, &H2_Pose, &H2_Vel);
331 }
332
333 // Jacobian w.r.t. IMUBias1
334 if (H3){
335 Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(
336 std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
337 this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
338 Bias1);
339 Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(
340 std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
341 this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
342 Bias1);
343 *H3 = stack(nrMatrices: 2, &H3_Pose, &H3_Vel);
344 }
345
346 // Jacobian w.r.t. Pose2
347 if (H4){
348 Matrix H4_Pose = numericalDerivative11<POSE, POSE>(
349 std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
350 this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
351 Pose2);
352 Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(
353 std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
354 this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
355 Pose2);
356 *H4 = stack(nrMatrices: 2, &H4_Pose, &H4_Vel);
357 }
358
359 // Jacobian w.r.t. Vel2
360 if (H5){
361 if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
362 Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(
363 std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
364 this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
365 Vel2);
366 Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(
367 std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
368 this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
369 Vel2);
370 *H5 = stack(nrMatrices: 2, &H5_Pose, &H5_Vel);
371 }
372
373 Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
374 Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
375
376 return concatVectors(nrVectors: 2, &ErrPoseVector, &ErrVelVector);
377 }
378
379
380
381 static inline POSE PredictPoseFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
382 const Vector& delta_pos_in_t0, const Vector3& delta_angles,
383 double dt12, const Vector world_g, const Vector world_rho,
384 const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
385 const std::optional<IMUBIAS>& Bias_initial = {}) {
386
387
388 // Correct delta_pos_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_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(startRow: 4,startCol: 9,blockRows: 3,blockCols: 3);
397 Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(startRow: 4,startCol: 12,blockRows: 3,blockCols: 3);
398 Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(startRow: 0,startCol: 12,blockRows: 3,blockCols: 3);
399
400 /* Position term */
401 Vector delta_pos_in_t0_corrected = delta_pos_in_t0 + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
402
403 /* Rotation term */
404 Vector delta_angles_corrected = delta_angles + J_angles_wrt_BiasGyro*delta_BiasGyro;
405 // Another alternative:
406 // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
407
408 return predictPose_inertial(Pose1, Vel1, delta_pos_in_t0: delta_pos_in_t0_corrected, delta_angles: delta_angles_corrected, dt12, world_g, world_rho, world_omega_earth);
409 }
410
411 static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
412 const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho,
413 const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
414 const std::optional<IMUBIAS>& Bias_initial = {}) {
415
416 // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
417 Vector delta_BiasAcc = Bias1.accelerometer();
418 Vector delta_BiasGyro = Bias1.gyroscope();
419 if (Bias_initial){
420 delta_BiasAcc -= Bias_initial->accelerometer();
421 delta_BiasGyro -= Bias_initial->gyroscope();
422 }
423
424 Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(startRow: 6,startCol: 9,blockRows: 3,blockCols: 3);
425 Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall.block(startRow: 6,startCol: 12,blockRows: 3,blockCols: 3);
426
427 Vector delta_vel_in_t0_corrected = delta_vel_in_t0 + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
428
429 return predictVelocity_inertial(Pose1, Vel1, delta_vel_in_t0: delta_vel_in_t0_corrected, dt12, world_g, world_rho, world_omega_earth);
430 }
431
432 static inline void PredictFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2,
433 const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
434 double dt12, const Vector world_g, const Vector world_rho,
435 const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
436 const std::optional<IMUBIAS>& Bias_initial = {}) {
437
438 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);
439 Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
440 }
441
442
443 static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
444 Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
445 const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
446 Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
447 std::optional<POSE> p_body_P_sensor = {}){
448 // Note: all delta terms refer to an IMU\sensor system at t0
449 // Note: Earth-related terms are not accounted here but are incorporated in predict functions.
450
451 POSE body_P_sensor = POSE();
452 bool flag_use_body_P_sensor = false;
453 if (p_body_P_sensor){
454 body_P_sensor = *p_body_P_sensor;
455 flag_use_body_P_sensor = true;
456 }
457
458 delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
459 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);
460 delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
461
462 delta_t += msr_dt;
463
464 // Update EquivCov_Overall
465 Matrix Z_3x3 = Z_3x3;
466 Matrix I_3x3 = I_3x3;
467
468 Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(
469 h: std::bind(f: &PreIntegrateIMUObservations_delta_pos, args: msr_dt,
470 args: std::placeholders::_1, args&: delta_vel_in_t0),
471 x: delta_pos_in_t0);
472 Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(
473 h: std::bind(f: &PreIntegrateIMUObservations_delta_pos, args: msr_dt,
474 args&: delta_pos_in_t0, args: std::placeholders::_1),
475 x: delta_vel_in_t0);
476 Matrix H_pos_angles = Z_3x3;
477 Matrix H_pos_bias = collect(nrMatrices: 2, &Z_3x3, &Z_3x3);
478
479 Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(
480 std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
481 msr_acc_t, msr_dt, delta_angles, std::placeholders::_1,
482 flag_use_body_P_sensor, body_P_sensor, Bias_t0),
483 delta_vel_in_t0);
484 Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(
485 std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
486 msr_acc_t, msr_dt, std::placeholders::_1, delta_vel_in_t0,
487 flag_use_body_P_sensor, body_P_sensor, Bias_t0),
488 delta_angles);
489 Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(
490 std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
491 msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0,
492 flag_use_body_P_sensor, body_P_sensor,
493 std::placeholders::_1),
494 Bias_t0);
495 Matrix H_vel_pos = Z_3x3;
496
497 Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(
498 std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
499 msr_dt, std::placeholders::_1, flag_use_body_P_sensor,
500 body_P_sensor, Bias_t0),
501 delta_angles);
502 Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(
503 std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
504 msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor,
505 std::placeholders::_1),
506 Bias_t0);
507 Matrix H_angles_pos = Z_3x3;
508 Matrix H_angles_vel = Z_3x3;
509
510 Matrix F_angles = collect(nrMatrices: 4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
511 Matrix F_pos = collect(nrMatrices: 4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
512 Matrix F_vel = collect(nrMatrices: 4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
513 Matrix F_bias_a = collect(nrMatrices: 5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
514 Matrix F_bias_g = collect(nrMatrices: 5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
515 Matrix F = stack(nrMatrices: 5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
516
517
518 noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model: model_continuous_overall, delta_t: msr_dt );
519 Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse();
520
521 EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
522 // Luca: force identity covariance matrix (for testing purposes)
523 // EquivCov_Overall = Matrix::Identity(15,15);
524
525 // Update Jacobian_wrt_t0_Overall
526 Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
527 }
528
529 static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
530 const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
531
532 // Note: all delta terms refer to an IMU\sensor system at t0
533 // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
534
535 return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
536 }
537
538
539
540 static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
541 const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
542 IMUBIAS Bias_t0 = IMUBIAS()){
543
544 // Note: all delta terms refer to an IMU\sensor system at t0
545
546 // Calculate the corrected measurements using the Bias object
547 Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
548 Vector body_t_a_body;
549 if (flag_use_body_P_sensor){
550 Matrix body_R_sensor = body_P_sensor.rotation().matrix();
551
552 Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
553
554 Vector body_omega_body = body_R_sensor * GyroCorrected;
555 Matrix body_omega_body__cross = skewSymmetric(w: body_omega_body);
556
557 body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
558 } else{
559 body_t_a_body = AccCorrected;
560 }
561
562 Rot3 R_t_to_t0 = Rot3::Expmap(v: delta_angles);
563
564 return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
565 }
566
567
568 static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
569 const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
570 IMUBIAS Bias_t0 = IMUBIAS()){
571
572 // Note: all delta terms refer to an IMU\sensor system at t0
573
574 // Calculate the corrected measurements using the Bias object
575 Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
576
577 Vector body_t_omega_body;
578 if (flag_use_body_P_sensor){
579 body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
580 } else {
581 body_t_omega_body = GyroCorrected;
582 }
583
584 Rot3 R_t_to_t0 = Rot3::Expmap(v: delta_angles);
585
586 R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( v: body_t_omega_body*msr_dt );
587 return Rot3::Logmap(R: R_t_to_t0);
588 }
589
590
591 static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
592 const noiseModel::Gaussian::shared_ptr& gaussian_process){
593
594 Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
595 Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
596 Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
597
598 cov_process.block(startRow: 0,startCol: 0, blockRows: 3,blockCols: 3) += cov_gyro;
599 cov_process.block(startRow: 6,startCol: 6, blockRows: 3,blockCols: 3) += cov_acc;
600
601 return noiseModel::Gaussian::Covariance(covariance: cov_process);
602 }
603
604 static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
605 const noiseModel::Gaussian::shared_ptr& gaussian_process,
606 Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){
607
608 cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
609 cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
610 cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
611 }
612
613 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,
614 Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
615
616 Matrix ENU_to_NED = (Matrix(3, 3) <<
617 0.0, 1.0, 0.0,
618 1.0, 0.0, 0.0,
619 0.0, 0.0, -1.0).finished();
620
621 Matrix NED_to_ENU = (Matrix(3, 3) <<
622 0.0, 1.0, 0.0,
623 1.0, 0.0, 0.0,
624 0.0, 0.0, -1.0).finished();
625
626 // Convert incoming parameters to ENU
627 Vector Pos_ENU = NED_to_ENU * Pos_NED;
628 Vector Vel_ENU = NED_to_ENU * Vel_NED;
629 Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
630
631 // Call ENU version
632 Vector g_ENU;
633 Vector rho_ENU;
634 Vector omega_earth_ENU;
635 Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
636
637 // Convert output to NED
638 g_NED = ENU_to_NED * g_ENU;
639 rho_NED = ENU_to_NED * rho_ENU;
640 omega_earth_NED = ENU_to_NED * omega_earth_ENU;
641 }
642
643 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,
644 Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
645 double R0 = 6.378388e6;
646 double e = 1/297;
647 double Re( R0*( 1-e*(sin( x: LatLonHeight_IC(0) ))*(sin( x: LatLonHeight_IC(0) )) ) );
648
649 // Calculate current lat, lon
650 Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
651 double delta_lat(delta_Pos_ENU(1)/Re);
652 double delta_lon(delta_Pos_ENU(0)/(Re*cos(x: LatLonHeight_IC(0))));
653 double lat_new(LatLonHeight_IC(0) + delta_lat);
654 double lon_new(LatLonHeight_IC(1) + delta_lon);
655
656 // Rotation of lon about z axis
657 Rot3 C1(cos(x: lon_new), sin(x: lon_new), 0.0,
658 -sin(x: lon_new), cos(x: lon_new), 0.0,
659 0.0, 0.0, 1.0);
660
661 // Rotation of lat about y axis
662 Rot3 C2(cos(x: lat_new), 0.0, sin(x: lat_new),
663 0.0, 1.0, 0.0,
664 -sin(x: lat_new), 0.0, cos(x: lat_new));
665
666 Rot3 UEN_to_ENU(0, 1, 0,
667 0, 0, 1,
668 1, 0, 0);
669
670 Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
671
672 Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5));
673 omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
674
675 // Calculating g
676 double height(LatLonHeight_IC(2));
677 double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
678 double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
679 double e2( pow(x: ECCENTRICITY,y: 2) );
680 double den( 1-e2*pow(x: sin(x: lat_new),y: 2) );
681 double Rm( (EQUA_RADIUS*(1-e2))/( pow(x: den,y: (3/2)) ) );
682 double Rp( EQUA_RADIUS/( sqrt(x: den) ) );
683 double Ro( sqrt(x: Rp*Rm) ); // mean earth radius of curvature
684 double g0( 9.780318*( 1 + 5.3024e-3 * pow(x: sin(x: lat_new),y: 2) - 5.9e-6 * pow(x: sin(x: 2*lat_new),y: 2) ) );
685 double g_calc( g0/( pow(x: 1 + height/Ro, y: 2) ) );
686 g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished();
687
688
689 // Calculate rho
690 double Ve( Vel_ENU(0) );
691 double Vn( Vel_ENU(1) );
692 double rho_E = -Vn/(Rm + height);
693 double rho_N = Ve/(Rp + height);
694 double rho_U = Ve*tan(x: lat_new)/(Rp + height);
695 rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished();
696 }
697
698 static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
699 /* Q_d (approx)= Q * delta_t */
700 /* In practice, square root of the information matrix is represented, so that:
701 * R_d (approx)= R / sqrt(delta_t)
702 * */
703 return noiseModel::Gaussian::SqrtInformation(R: model->R()/sqrt(x: delta_t));
704 }
705private:
706
707#if GTSAM_ENABLE_BOOST_SERIALIZATION
708 /** Serialization function */
709 friend class boost::serialization::access;
710 template<class ARCHIVE>
711 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
712 ar & boost::serialization::make_nvp("NonlinearFactor2",
713 boost::serialization::base_object<Base>(*this));
714 }
715#endif
716
717
718
719}; // \class EquivInertialNavFactor_GlobalVel
720
721} /// namespace gtsam
722