Mechanization_bRn2.cpp
Go to the documentation of this file.
1 
8 #include "Mechanization_bRn2.h"
9 
10 namespace gtsam {
11 
12 /* ************************************************************************* */
14  const std::list<Vector>& F, const double g_e, bool flat) {
15  Matrix Umat = (Matrix(3, U.size()) << concatVectors(U)).finished();
16  Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F)).finished();
17 
18  return initialize(Umat, Fmat, g_e, flat);
19 }
20 
21 
22 /* ************************************************************************* */
24  const Matrix& F, const double g_e, bool flat) {
25 
26  // estimate gyro bias by averaging gyroscope readings (10.62)
27  Vector3 x_g = U.rowwise().mean();
28  Vector3 meanF = F.rowwise().mean();
29 
30  // estimate gravity
31  Vector3 b_g;
32  if(g_e == 0) {
33  if (flat)
34  // acceleration measured is along the z-axis.
35  b_g = (Vector3(3) << 0.0, 0.0, meanF.norm()).finished();
36  else
37  // acceleration measured is the opposite of gravity (10.13)
38  b_g = -meanF;
39  } else {
40  if (flat)
41  // gravity is downward along the z-axis since we are flat on the ground
42  b_g = (Vector3(3) << 0.0,0.0,g_e).finished();
43  else
44  // normalize b_g and attribute remainder to biases
45  b_g = - g_e * meanF/meanF.norm();
46  }
47 
48  // estimate accelerometer bias
49  Vector3 x_a = meanF + b_g;
50 
51  // initialize roll, pitch (eqns. 10.14, 10.15)
52  double g1=b_g(0);
53  double g2=b_g(1);
54  double g3=b_g(2);
55 
56  // Farrell08book eqn. 10.14
57  double roll = atan2(g2, g3);
58  // Farrell08book eqn. 10.15
59  double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3));
60  double yaw = 0;
61  // This returns body-to-nav nRb
62  Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse();
63 
64  return Mechanization_bRn2(bRn, x_g, x_a);
65 }
66 
67 /* ************************************************************************* */
69  Vector3 rho = dx.segment<3>(0);
70 
71  Rot3 delta_nRn = Rot3::Rodrigues(rho);
72  Rot3 bRn = bRn_ * delta_nRn;
73 
74  Vector3 x_g = x_g_ + dx.segment<3>(3);
75  Vector3 x_a = x_a_ + dx.segment<3>(6);
76 
77  return Mechanization_bRn2(bRn, x_g, x_a);
78 }
79 
80 /* ************************************************************************* */
82  const double dt) const {
83  // integrate rotation nRb based on gyro measurement u and bias x_g
84 
85 #ifndef MODEL_NAV_FRAME_ROTATION
86  // get body to inertial (measured in b) from gyro, subtract bias
87  Vector3 b_omega_ib = u - x_g_;
88 
89  // nav to inertial due to Earth's rotation and our movement on Earth surface
90  // TODO: figure out how to do this if we need it
91  Vector3 b_omega_in; b_omega_in.setZero();
92 
93  // calculate angular velocity on bRn measured in body frame
94  Vector3 b_omega_bn = b_omega_in - b_omega_ib;
95 #else
96  // Assume a non-rotating nav frame (probably an approximation)
97  // calculate angular velocity on bRn measured in body frame
98  Vector3 b_omega_bn = x_g_ - u;
99 #endif
100 
101  // convert to navigation frame
102  Rot3 nRb = bRn_.inverse();
103  Vector3 n_omega_bn = nRb * b_omega_bn;
104 
105  // integrate bRn using exponential map, assuming constant over dt
106  Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt);
107  Rot3 bRn = bRn_.compose(delta_nRn);
108 
109  // implicit updating of biases (variables just do not change)
110  // x_g = x_g;
111  // x_a = x_a;
112  return Mechanization_bRn2(bRn, x_g_, x_a_);
113 }
114 
115 } /* namespace gtsam */
const Vector3 & x_a() const
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
static Mechanization_bRn2 initialize(const Matrix &U, const Matrix &F, const double g_e=0, bool flat=false)
Matrix version of initialize.
Key F(std::uint64_t j)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 x_a_
accelerometer bias
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Vector3 b_g(double g_e) const
gravity in the body frame
Rot3 bRn_
rotation from nav to body
Mechanization_bRn2 integrate(const Vector3 &u, const double dt) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
const double dt
double g_e
Definition: testAHRS.cpp:21
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Mechanization_bRn2(const Rot3 &initial_bRn=Rot3(), const Vector3 &initial_x_g=Z_3x1, const Vector3 &initial_x_a=Z_3x1)
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
const Rot3 nRb
const Rot3 & bRn() const
Class compose(const Class &g) const
Definition: Lie.h:48
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
traits
Definition: chartTesting.h:28
const Vector3 & x_g() const
Vector3 x_g_
gyroscope bias
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
static Mechanization_bRn2 initializeVector(const std::list< Vector > &U, const std::list< Vector > &F, const double g_e=0, bool flat=false)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector concatVectors(const std::list< Vector > &vs)
Definition: Vector.cpp:301
Pose3 g2(g1.expmap(h *V1_g1))
Mechanization_bRn2 correct(const Vector9 &dx) const


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