1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file InertialNavFactor_GlobalVelocity.h
14 * @author Vadim Indelman, Stephen Williams
15 * @brief Inertial navigation factor (velocity in the global frame)
16 * @date Sept 13, 2012
17 **/
18
19#pragma once
20
21#include <gtsam/nonlinear/NonlinearFactor.h>
22#include <gtsam/linear/NoiseModel.h>
23#include <gtsam/geometry/Rot3.h>
24#include <gtsam/base/Matrix.h>
25
26// Using numerical derivative to calculate d(Pose3::Expmap)/dw
27#include <gtsam/base/numericalDerivative.h>
28
29#include <ostream>
30
31namespace gtsam {
32
33/*
34 * NOTES:
35 * =====
36 * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame.
37 * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames.
38 * - Camera and IMU frames are identical
39 * - The user should specify a continuous equivalent noise covariance, which can be calculated using
40 * the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance
41 * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
42 * discrete form using the supplied delta_t between sub-sequential measurements.
43 * - Earth-rate correction:
44 * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
45 * frame (Local-Level system: ENU or NED, see above).
46 * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
47 * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
48 * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
49 *
50 * - Frame Notation:
51 * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
52 * So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor
53 * And the transformation from the body frame to the world frame would be: world_P_body
54 * This allows visual chaining. For example, converting the sensed angular velocity of the IMU
55 * (angular velocity of the sensor in the sensor frame) into the world frame can be performed as:
56 * world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor
57 *
58 *
59 * - Common Quantity Types
60 * P : pose/3d transformation
61 * R : rotation
62 * omega : angular velocity
63 * t : translation
64 * v : velocity
65 * a : acceleration
66 *
67 * - Common Frames
68 * sensor : the coordinate system attached to the sensor origin
69 * body : the coordinate system attached to body/inertial frame.
70 * Unless an optional frame transformation is provided, the
71 * sensor frame and the body frame will be identical
72 * world : the global/world coordinate frame. This is assumed to be
73 * a tangent plane to the earth's surface somewhere near the
74 * vehicle
75 */
76template<class POSE, class VELOCITY, class IMUBIAS>
77class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
78
79private:
80
81 typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
82 typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
83
84 Vector measurement_acc_;
85 Vector measurement_gyro_;
86 double dt_;
87
88 Vector world_g_;
89 Vector world_rho_;
90 Vector world_omega_earth_;
91
92 std::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
93
94public:
95
96 // Provide access to the Matrix& version of evaluateError:
97 using Base::evaluateError;
98
99 // shorthand for a smart pointer to a factor
100 typedef typename std::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
101
102 /** default constructor - only use for serialization */
103 InertialNavFactor_GlobalVelocity() {}
104
105 /** Constructor */
106 InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
107 const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
108 const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, std::optional<POSE> body_P_sensor = {}) :
109 Base(calc_descrete_noise_model(model: model_continuous, delta_t: measurement_dt ),
110 Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
111 dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }
112
113 ~InertialNavFactor_GlobalVelocity() override {}
114
115 /** implement functions needed for Testable */
116
117 /** print */
118 void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
119 std::cout << s << "("
120 << keyFormatter(this->key1()) << ","
121 << keyFormatter(this->key2()) << ","
122 << keyFormatter(this->key3()) << ","
123 << keyFormatter(this->key4()) << ","
124 << keyFormatter(this->key5()) << "\n";
125 std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
126 std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
127 std::cout << "dt: " << this->dt_ << std::endl;
128 std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
129 std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
130 std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
131 if(this->body_P_sensor_)
132 this->body_P_sensor_->print(" sensor pose in body frame: ");
133 this->noiseModel_->print(" noise model");
134 }
135
136 /** equals */
137 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
138 const This *e = dynamic_cast<const This*> (&expected);
139 return e != nullptr && Base::equals(*e, tol)
140 && (measurement_acc_ - e->measurement_acc_).norm() < tol
141 && (measurement_gyro_ - e->measurement_gyro_).norm() < tol
142 && (dt_ - e->dt_) < tol
143 && (world_g_ - e->world_g_).norm() < tol
144 && (world_rho_ - e->world_rho_).norm() < tol
145 && (world_omega_earth_ - e->world_omega_earth_).norm() < tol
146 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
147 }
148
149 POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
150 // Calculate the corrected measurements using the Bias object
151 Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
152
153 const POSE& world_P1_body = Pose1;
154 const VELOCITY& world_V1_body = Vel1;
155
156 // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
157 Vector body_omega_body;
158 if(body_P_sensor_) {
159 body_omega_body = body_P_sensor_->rotation().matrix() * GyroCorrected;
160 } else {
161 body_omega_body = GyroCorrected;
162 }
163
164 // Convert earth-related terms into the body frame
165 Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
166 Vector body_rho = body_R_world * world_rho_;
167 Vector body_omega_earth = body_R_world * world_omega_earth_;
168
169 // Correct for earth-related terms
170 body_omega_body -= body_rho + body_omega_earth;
171
172 // The velocity is in the global frame, so composing Pose1 with v*dt is incorrect
173 return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_omega_body*dt_), Pose1.translation() + typename POSE::Translation(world_V1_body*dt_));
174 }
175
176 VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
177 // Calculate the corrected measurements using the Bias object
178 Vector AccCorrected(Bias1.correctAccelerometer(measurement_acc_));
179
180 const POSE& world_P1_body = Pose1;
181 const VELOCITY& world_V1_body = Vel1;
182
183 // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
184 Vector body_a_body, body_omega_body;
185 if(body_P_sensor_) {
186 Matrix body_R_sensor = body_P_sensor_->rotation().matrix();
187
188 Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
189 body_omega_body = body_R_sensor * GyroCorrected;
190 Matrix body_omega_body__cross = skewSymmetric(w: body_omega_body);
191 body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation();
192 } else {
193 body_a_body = AccCorrected;
194 }
195
196 // Correct for earth-related terms
197 Vector world_a_body = world_P1_body.rotation().matrix() * body_a_body + world_g_ - 2*skewSymmetric(w: world_rho_ + world_omega_earth_)*world_V1_body;
198
199 // Calculate delta in the body frame
200 VELOCITY VelDelta(world_a_body*dt_);
201
202 // Predict
203 return Vel1 + VelDelta;
204 }
205
206 void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
207 Pose2 = predictPose(Pose1, Vel1, Bias1);
208 Vel2 = predictVelocity(Pose1, Vel1, Bias1);
209 }
210
211 POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
212 // Predict
213 POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
214
215 // Calculate error
216 return Pose2.between(Pose2Pred);
217 }
218
219 VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
220 // Predict
221 VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
222
223 // Calculate error
224 return Vel2Pred - Vel2;
225 }
226
227 /** implement functions needed to derive from Factor */
228 Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
229 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
230 OptionalMatrixType H5) const override {
231
232 // TODO: Write analytical derivative calculations
233 // Jacobian w.r.t. Pose1
234 if (H1){
235 Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(
236 std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
237 this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
238 Pose1);
239 Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
240 std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
241 this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
242 Pose1);
243 *H1 = stack(nrMatrices: 2, &H1_Pose, &H1_Vel);
244 }
245
246 // Jacobian w.r.t. Vel1
247 if (H2){
248 if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
249 Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
250 std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
251 this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
252 Vel1);
253 Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
254 std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
255 this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
256 Vel1);
257 *H2 = stack(nrMatrices: 2, &H2_Pose, &H2_Vel);
258 }
259
260 // Jacobian w.r.t. IMUBias1
261 if (H3){
262 Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(
263 std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
264 this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
265 Bias1);
266 Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(
267 std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
268 this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
269 Bias1);
270 *H3 = stack(nrMatrices: 2, &H3_Pose, &H3_Vel);
271 }
272
273 // Jacobian w.r.t. Pose2
274 if (H4){
275 Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(
276 std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
277 this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
278 Pose2);
279 Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
280 std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
281 this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
282 Pose2);
283 *H4 = stack(nrMatrices: 2, &H4_Pose, &H4_Vel);
284 }
285
286 // Jacobian w.r.t. Vel2
287 if (H5){
288 if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
289 Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
290 std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
291 this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
292 Vel2);
293 Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
294 std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
295 this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
296 Vel2);
297 *H5 = stack(nrMatrices: 2, &H5_Pose, &H5_Vel);
298 }
299
300 Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
301 Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
302
303 return concatVectors(nrVectors: 2, &ErrPoseVector, &ErrVelVector);
304 }
305
306 static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
307 const noiseModel::Gaussian::shared_ptr& gaussian_process){
308
309 Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
310 Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
311 Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
312
313 cov_process.block(startRow: 0,startCol: 0, blockRows: 3,blockCols: 3) += cov_gyro;
314 cov_process.block(startRow: 6,startCol: 6, blockRows: 3,blockCols: 3) += cov_acc;
315
316 return noiseModel::Gaussian::Covariance(covariance: cov_process);
317 }
318
319 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,
320 Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
321
322 Matrix ENU_to_NED = (Matrix(3, 3) <<
323 0.0, 1.0, 0.0,
324 1.0, 0.0, 0.0,
325 0.0, 0.0, -1.0).finished();
326
327 Matrix NED_to_ENU = (Matrix(3, 3) <<
328 0.0, 1.0, 0.0,
329 1.0, 0.0, 0.0,
330 0.0, 0.0, -1.0).finished();
331
332 // Convert incoming parameters to ENU
333 Vector Pos_ENU = NED_to_ENU * Pos_NED;
334 Vector Vel_ENU = NED_to_ENU * Vel_NED;
335 Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
336
337 // Call ENU version
338 Vector g_ENU;
339 Vector rho_ENU;
340 Vector omega_earth_ENU;
341 Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
342
343 // Convert output to NED
344 g_NED = ENU_to_NED * g_ENU;
345 rho_NED = ENU_to_NED * rho_ENU;
346 omega_earth_NED = ENU_to_NED * omega_earth_ENU;
347 }
348
349 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,
350 Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
351 double R0 = 6.378388e6;
352 double e = 1/297;
353 double Re( R0*( 1-e*(sin( x: LatLonHeight_IC(0) ))*(sin( x: LatLonHeight_IC(0) )) ) );
354
355 // Calculate current lat, lon
356 Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
357 double delta_lat(delta_Pos_ENU(1)/Re);
358 double delta_lon(delta_Pos_ENU(0)/(Re*cos(x: LatLonHeight_IC(0))));
359 double lat_new(LatLonHeight_IC(0) + delta_lat);
360 double lon_new(LatLonHeight_IC(1) + delta_lon);
361
362 // Rotation of lon about z axis
363 Rot3 C1(cos(x: lon_new), sin(x: lon_new), 0.0,
364 -sin(x: lon_new), cos(x: lon_new), 0.0,
365 0.0, 0.0, 1.0);
366
367 // Rotation of lat about y axis
368 Rot3 C2(cos(x: lat_new), 0.0, sin(x: lat_new),
369 0.0, 1.0, 0.0,
370 -sin(x: lat_new), 0.0, cos(x: lat_new));
371
372 Rot3 UEN_to_ENU(0, 1, 0,
373 0, 0, 1,
374 1, 0, 0);
375
376 Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
377
378 Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5));
379 omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
380
381 // Calculating g
382 double height(LatLonHeight_IC(2));
383 double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
384 double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
385 double e2( pow(x: ECCENTRICITY,y: 2) );
386 double den( 1-e2*pow(x: sin(x: lat_new),y: 2) );
387 double Rm( (EQUA_RADIUS*(1-e2))/( pow(x: den,y: (3/2)) ) );
388 double Rp( EQUA_RADIUS/( sqrt(x: den) ) );
389 double Ro( sqrt(x: Rp*Rm) ); // mean earth radius of curvature
390 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) ) );
391 double g_calc( g0/( pow(x: 1 + height/Ro, y: 2) ) );
392 g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished();
393
394
395 // Calculate rho
396 double Ve( Vel_ENU(0) );
397 double Vn( Vel_ENU(1) );
398 double rho_E = -Vn/(Rm + height);
399 double rho_N = Ve/(Rp + height);
400 double rho_U = Ve*tan(x: lat_new)/(Rp + height);
401 rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished();
402 }
403
404 static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
405 /* Q_d (approx)= Q * delta_t */
406 /* In practice, square root of the information matrix is represented, so that:
407 * R_d (approx)= R / sqrt(delta_t)
408 * */
409 return noiseModel::Gaussian::SqrtInformation(R: model->R()/std::sqrt(x: delta_t));
410 }
411
412private:
413
414#if GTSAM_ENABLE_BOOST_SERIALIZATION
415 /** Serialization function */
416 friend class boost::serialization::access;
417 template<class ARCHIVE>
418 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
419 ar & boost::serialization::make_nvp("NonlinearFactor2",
420 boost::serialization::base_object<Base>(*this));
421 }
422#endif
423
424}; // \class InertialNavFactor_GlobalVelocity
425
426/// traits
427template<class POSE, class VELOCITY, class IMUBIAS>
428struct traits<InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> > :
429 public Testable<InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> > {
430};
431
432} /// namespace aspn
433