1/**
2 * @file Mechanization_bRn2.cpp
3 * @date Jan 25, 2012
4 * @author Chris Beall
5 * @author Frank Dellaert
6 */
7
8#include "Mechanization_bRn2.h"
9
10namespace gtsam {
11
12/* ************************************************************************* */
13Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
14 const std::list<Vector>& F, const double g_e, bool flat) {
15 Matrix Umat = (Matrix(3, U.size()) << concatVectors(vs: U)).finished();
16 Matrix Fmat = (Matrix(3, F.size()) << concatVectors(vs: F)).finished();
17
18 return initialize(U: Umat, F: Fmat, g_e, flat);
19}
20
21
22/* ************************************************************************* */
23Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
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(y: g2, x: g3);
58 // Farrell08book eqn. 10.15
59 double pitch = atan2(y: -g1, x: sqrt(x: g2 * g2 + g3 * g3));
60 double yaw = 0;
61 // This returns body-to-nav nRb
62 Rot3 bRn = Rot3::Ypr(y: yaw, p: pitch, r: roll).inverse();
63
64 return Mechanization_bRn2(bRn, x_g, x_a);
65}
66
67/* ************************************************************************* */
68Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const {
69 Vector3 rho = dx.segment<3>(start: 0);
70
71 Rot3 delta_nRn = Rot3::Rodrigues(w: rho);
72 Rot3 bRn = bRn_ * delta_nRn;
73
74 Vector3 x_g = x_g_ + dx.segment<3>(start: 3);
75 Vector3 x_a = x_a_ + dx.segment<3>(start: 6);
76
77 return Mechanization_bRn2(bRn, x_g, x_a);
78}
79
80/* ************************************************************************* */
81Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u,
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(w: n_omega_bn*dt);
107 Rot3 bRn = bRn_.compose(g: 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 */
116