ImuBias.cpp
Go to the documentation of this file.
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 
18 #include "ImuBias.h"
19 
20 #include <gtsam/geometry/Point3.h>
21 #include <iostream>
22 
23 namespace gtsam {
24 
26 namespace imuBias {
27 
28 /*
29  * NOTES:
30  * - Earth-rate correction:
31  * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
32  * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
33  * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
34  * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
35  *
36  * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
37  */
38 // // H1: Jacobian w.r.t. IMUBias
39 // // H2: Jacobian w.r.t. pose
40 // Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
41 // Matrix* H1=nullptr, Matrix* H2=nullptr) const {
42 //
43 // Matrix R_G_to_I( pose.rotation().matrix().transpose() );
44 // Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;
45 //
46 // if (H1){
47 // Matrix zeros3_3(zeros(3,3));
48 // Matrix m_eye3(-eye(3));
49 //
50 // *H1 = collect(2, &zeros3_3, &m_eye3);
51 // }
52 //
53 // if (H2){
54 // Matrix zeros3_3(zeros(3,3));
55 // Matrix H = -skewSymmetric(w_earth_rate_I);
56 //
57 // *H2 = collect(2, &H, &zeros3_3);
58 // }
59 //
60 // //TODO: Make sure H2 is correct.
61 //
62 // return measurement - biasGyro_ - w_earth_rate_I;
63 //
66 // }
68 std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
69  os << "acc = " << bias.accelerometer().transpose();
70  os << " gyro = " << bias.gyroscope().transpose();
71  return os;
72 }
73 
75 void ConstantBias::print(const std::string& s) const {
76  std::cout << s << *this << std::endl;
77 }
78 
79 } // namespace imuBias
80 
81 } // namespace gtsam
82 
void print(const std::string &s="") const
print with optional string
Definition: ImuBias.cpp:75
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition: ImuBias.cpp:68
const Vector3 & accelerometer() const
Definition: ImuBias.h:66
RealScalar s
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
3D Point
const Vector3 & gyroscope() const
Definition: ImuBias.h:71


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:21