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
|
#ifndef state_INC
#define state_INC
#include <Eigen/Dense>
#include <list>
#include <vector>
#include "body.h"
#include "camera.h"
#include "feature.h"
#include "types.h"
#define MAXFEATURES 30
//#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
#define FULLS // Comment out to treat each measurement independently.
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Dynamic;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using std::cout;
using std::cerr;
using std::endl;
/*
* =====================================================================================
* Class: State
* Description:
* =====================================================================================
*/
class State
{
public:
/* ==================== LIFECYCLE ======================================= */
State (); /* constructor */
/* ==================== ACCESSORS ======================================= */
bool exists(int id);
int rowById(int id);
Feature *featureById(int id);
MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
int Hrows( const std::vector<measurement_t> &z );
MatrixXd Q(double dt);
MatrixXd R( const std::vector<measurement_t> &z );
Matrix<double,9,9> Pxx();
Matrix<double,Dynamic,6> L();
/* ==================== 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 Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
void position_covariance(const Matrix<double,3,3> &p);
std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
void velocity_covariance(const Matrix<double,3,3> &p);
void vel(const Matrix<double,3,1> &v);
void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
/* ==================== OPERATORS ======================================= */
void unicsv();
protected:
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
private:
/* ==================== METHODS ======================================= */
void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
void expandP();
void shrinkP( int i );
/* ==================== DATA MEMBERS ======================================= */
Body *body;
//Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
Matrix<double, Dynamic, Dynamic> P;
char utm_c;
int utm_i;
std::list<Feature *> features;
}; /* ----- end of class State ----- */
#endif /* ----- #ifndef state_INC ----- */
|
