1/**
2 * @file Mechanization_bRn2.h
3 * @date Jan 25, 2012
4 * @author Chris Beall
5 * @author Frank Dellaert
6 */
7
8#pragma once
9
10#include <gtsam/geometry/Rot3.h>
11#include <gtsam/base/Vector.h>
12#include <gtsam_unstable/dllexport.h>
13#include <list>
14
15namespace gtsam {
16
17class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 {
18
19private:
20 Rot3 bRn_; ///< rotation from nav to body
21 Vector3 x_g_; ///< gyroscope bias
22 Vector3 x_a_; ///< accelerometer bias
23
24public:
25
26 /**
27 * Constructor
28 * @param initial_bRn initial rotation from nav to body frame
29 * @param initial_x_g initial gyro bias
30 * @param r3 Z-axis of rotated frame
31 */
32 Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
33 const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) :
34 bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
35 }
36
37 /// Copy constructor
38 Mechanization_bRn2(const Mechanization_bRn2& other) = default;
39
40 Mechanization_bRn2& operator=(const Mechanization_bRn2& other) = default;
41
42 /// gravity in the body frame
43 Vector3 b_g(double g_e) const {
44 Vector3 n_g(0, 0, g_e);
45 return bRn_ * n_g;
46 }
47
48 const Rot3& bRn() const {return bRn_; }
49 const Vector3& x_g() const { return x_g_; }
50 const Vector3& x_a() const { return x_a_; }
51
52 /**
53 * Initialize the first Mechanization state
54 * @param U a list of gyro measurement vectors
55 * @param F a list of accelerometer measurement vectors
56 * @param g_e gravity magnitude
57 * @param flat flag saying whether this is a flat trim init
58 */
59 static Mechanization_bRn2 initializeVector(const std::list<Vector>& U,
60 const std::list<Vector>& F, const double g_e = 0, bool flat=false);
61
62 /// Matrix version of initialize
63 static Mechanization_bRn2 initialize(const Matrix& U,
64 const Matrix& F, const double g_e = 0, bool flat=false);
65
66 /**
67 * Correct AHRS full state given error state dx
68 * @param obj The current state
69 * @param dx The error used to correct and return a new state
70 */
71 Mechanization_bRn2 correct(const Vector9& dx) const;
72
73 /**
74 * Integrate to get new state
75 * @param obj The current state
76 * @param u gyro measurement to integrate
77 * @param dt time elapsed since previous state in seconds
78 */
79 Mechanization_bRn2 integrate(const Vector3& u, const double dt) const;
80
81 /// print
82 void print(const std::string& s = "") const {
83 bRn_.print(s: s + ".R");
84
85 std::cout << s + ".x_g" << x_g_ << std::endl;
86 std::cout << s + ".x_a" << x_a_ << std::endl;
87 }
88
89};
90
91} // namespace gtsam
92