1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
#ifndef body_INC
#define body_INC
#include <Eigen/Dense>
#include <iostream>
#include "types.h"
#define R_HEIGHT 2.5e-3 /* measurement noise of height measurement */
#define ACC_STD 28e-3
#define ANG_STD 9e-3
#define ACC_BIAS_STD 22e-3
#define ANG_BIAS_STD 3e-3
#define IMU_NOISE 800e-6 /* IMU process noise */
#define IMU_RANDOMWALK 50e-6 /* Bias process noise */
//#define DOCLAMP
#define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
#define MINHEIGHT -0.3 /* */
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::Quaterniond;
using std::cout;
using std::cerr;
using std::endl;
/*
* =====================================================================================
* Class: Body
* Description: State representation of body.
* =====================================================================================
*/
class Body
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* ==================== LIFECYCLE ======================================= */
Body (){ gravity_world << 0.,0.,9.80655; }; /* constructor */
/* ==================== ACCESSORS ======================================= */
#if STATESIZE==13
Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
Quaterniond qhat();
void qhat(const Quaterniond &q);
void normalizeQuaternion();
#else
Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
#endif
Matrix<double,STATESIZE,1> asVector();
void asVector(const Matrix<double,STATESIZE,1> &m);
void dx( const Matrix<double,STATESIZE,1> &del);
Matrix<double,1,STATESIZE> H();
Matrix<double,STATESIZE,STATESIZE> Q(double dt);
Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx);
Vector3d ned();
Vector3d vel();
Vector3d accelerometer_bias( );
/* ==================== MUTATORS ======================================= */
void accelerometer_bias( const Vector3d &b);
void clamp();
void pos( const UTM &utm);
void vel(const Matrix<double,3,1> &v);
/* ==================== OPERATORS ======================================= */
Matrix<double,1,1> h();
#if STATESIZE==13
void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt);
#else
void motionModel ( const Vector3d &acc, const Vector3d &ang,
const Quaterniond &q, const double dt);
#endif
Matrix<double,1,1> R();
Matrix<double,3,3> skewSymmetric(const Vector3d &x);
Matrix<double,4,4> omega(const Vector3d &x);
Matrix<double,4,3> qomega(const Quaterniond &q);
void unicsv();
protected:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
private:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
Matrix<double,STATESIZE,1> X;
char utm_c;
int utm_i;
Vector3d gravity_world;
}; /* ----- end of class Body ----- */
#endif /* ----- #ifndef body_INC ----- */
|
