gtsam
navigation
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
s
RealScalar s
Definition:
level1_cplx_impl.h:126
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
Point3.h
3D Point
os
ofstream os("timeSchurFactors.csv")
gtsam::imuBias::operator<<
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition:
ImuBias.cpp:68
gtsam::imuBias::ConstantBias::print
void print(const std::string &s="") const
print with optional string
Definition:
ImuBias.cpp:75
ImuBias.h
gtsam::imuBias::ConstantBias
Definition:
ImuBias.h:32
gtsam
traits
Definition:
SFMdata.h:40
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:44