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
|
#ifndef state_INC
#define state_INC
#include <Eigen/Dense>
#include <list>
#include <vector>
#include "body.h"
#include "feature.h"
#include "types.h"
#define MAXFEATURES 30
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Dynamic;
using Eigen::Vector3d;
using Eigen::Quaterniond;
/*
* =====================================================================================
* Class: State
* Description:
* =====================================================================================
*/
class State
{
public:
/* ==================== LIFECYCLE ======================================= */
State (); /* constructor */
/* ==================== ACCESSORS ======================================= */
bool exists(int id);
/* ==================== MUTATORS ======================================= */
void accelerometer_bias(const Vector3d &b);
void enu(const UTM &utm);
void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
void initializePi(int i, const Matrix<double,3,3> &Pi);
void motionModel(const Vector3d &acc, const Vector3d &ang,
const Quaterniond &q, const double dt);
void position_covariance(const Matrix<double,3,3> &p);
void velocity_covariance(const Matrix<double,3,3> &p);
void vel(const Matrix<double,3,1> &v);
void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
void update ( const Matrix<double,1,1> &z);
/* ==================== OPERATORS ======================================= */
void unicsv();
protected:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
private:
/* ==================== METHODS ======================================= */
void expandP(int Nnew);
/* ==================== DATA MEMBERS ======================================= */
Body *body;
Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
char utm_c;
int utm_i;
std::list<Feature *> features;
}; /* ----- end of class State ----- */
#endif /* ----- #ifndef state_INC ----- */
|
