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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
|
/*
* =====================================================================================
*
* Filename: state.cpp
*
* Description: A Class for managing body and features.
*
* Version: 1.0
* Created: 03/17/2017 07:55:56 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "state.h"
State::State ( )
{
body = new Body;
P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
P.block(6,6,3,3) = 1e-6*Matrix3d::Identity();
return ;
} /* ----- end of method State::State ----- */
void
State::vel ( const Matrix<double,3,1> &v )
{
body->vel(v);
return ;
} /* ----- end of method State::vel ----- */
void
State::enu ( const UTM &utm )
{
utm_c = utm.zone_c;
utm_i = utm.zone_i;
body->enu(utm);
return ;
} /* ----- end of method State::enu ----- */
void
State::update ( const Matrix<double,1,1> &z )
{
Matrix<double,1,9> H;
H=body->H();
Matrix<double,1,1> S;
S=body->S(P);
Matrix<double,9,1> K;
K = P*H.transpose()*S.inverse();
P = (Matrix<double,9,9>::Identity()-K*H)*P;
P = 0.5*(P+P.transpose());
Matrix<double,9,1> dx;
Matrix<double,1,1> h;
h = body->h();
dx = K*(z-h);
body->update(dx);
}
/*
*--------------------------------------------------------------------------------------
* Class: State
* Method: State :: Pkk1
* Description: Updates P_k|k-1
*--------------------------------------------------------------------------------------
*/
void
State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
{
Matrix<double,9,9> F,Q;
Q = body->Q(dt);
F = body->F(ang,q);
P = F*P*F.transpose()+Q;
// Enforce symmetry
P = 0.5*(P+P.transpose());
return ;
} /* ----- end of method State::Pkk1 ----- */
void
State::motionModel ( const Vector3d &acc, const Vector3d &ang,
const Quaterniond &q, const double dt)
{
Pkk1(ang,q,dt);
body->motionModel(acc,ang,q,dt);
return ;
} /* ----- end of method State::motionModel ----- */
void
State::position_covariance ( const Matrix<double,3,3> &p )
{
P.block(0,0,3,3) = p;
return ;
} /* ----- end of method State::position_covariance ----- */
void
State::velocity_covariance ( const Matrix<double,3,3> &p )
{
P.block(3,3,3,3) = p;
return ;
} /* ----- end of method State::velocity_covariance ----- */
void
State::unicsv ( )
{
body->unicsv();
return ;
} /* ----- end of method State::unicsv ----- */
void
State::accelerometer_bias ( const Vector3d &b)
{
body->accelerometer_bias(b);
return ;
} /* ----- end of method State::accelerometer_bias ----- */
|
