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
|
#ifndef body_INC
#define body_INC
#include <Eigen/Dense>
#include <iostream>
using Eigen::Matrix;
using Eigen::Vector3d;
using Eigen::Quaternion;
/*
* =====================================================================================
* Class: Body
* Description: State representation of body.
* =====================================================================================
*/
class Body
{
public:
/* ==================== LIFECYCLE ======================================= */
Body (){}; /* constructor */
/* ==================== ACCESSORS ======================================= */
/* ==================== MUTATORS ======================================= */
/* ==================== OPERATORS ======================================= */
void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc,
const Vector3d &ang, const Quaternion<double> &q, const double dt);
Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
void H(Matrix<double, 1,9> &H);
Matrix<double,1,1> h(const Matrix<double,9,1> &X);
Matrix<double,9,9> Q(double dt);
Matrix<double,1,1> R();
protected:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
private:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
}; /* ----- end of class Body ----- */
#endif /* ----- #ifndef body_INC ----- */
|
