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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
/*
* =====================================================================================
*
* Filename: body.cpp
*
* Description: Method definitions for body class.
*
* Version: 1.0
* Created: 03/17/2017 08:07:35 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include <Eigen/Dense>
#include "body.h"
Matrix<double,1,1>
Body::R()
{
Matrix<double,1,1> R;
R << 1e-8;
return R;
}
Matrix<double,9,9>
Body::Q (double dt)
{
Matrix<double,9,9> Q;
Q = Matrix<double,9,9>::Zero();
Q.block(0,0,3,3) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
Q.block(3,0,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
Q.block(0,3,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
Q.block(3,3,3,3) = dt*dt*Matrix<double,3,3>::Identity();
Q *= 800e-6;
Q.block(6,6,3,3) = dt*dt*5e-8*Matrix<double,3,3>::Identity();
return Q;
} /* ----- end of method Body::q ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: H
* Description: Writes the Jacobian of the measurement model into H.
*--------------------------------------------------------------------------------------
*/
void
Body::H ( Matrix<double,1,9> &H )
{
H = Matrix<double,1,9>::Zero();
H[0,2] = 1;
return ;
} /* ----- end of method Body::H ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: h
* Description: Writes the predicted measurement vector into h.
*--------------------------------------------------------------------------------------
*/
Matrix<double,1,1>
Body::h ( const Matrix<double,9,1> &X)
{
Matrix<double,1,1> h;
h[0] = X[2];
return h;
} /* ----- end of method Body::h_hat ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: F
* Description: Computes the Jacobian of the motion model of the body.
*--------------------------------------------------------------------------------------
*/
Matrix<double,9,9>
Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
{
Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
Matrix<double,3,3> Rbw(q.toRotationMatrix());
Matrix<double,3,3> W;
skewSymmetric(ang,W);
F.block(0,3,3,3) = Rbw;
F.block(3,3,3,3) = -W;
F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
return F;
} /* ----- end of method Body::F ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: motionModel
* Description: Propagates the motion model of the body into vector X.
*--------------------------------------------------------------------------------------
*/
void
Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
{
Vector3d bias(X.segment(6,3));
Vector3d gravity_world(0.,0.,9.80655);
Matrix<double,6,3> A;
Matrix<double,6,1> b;
A = Matrix<double,6,3>::Zero();
b = Matrix<double,6,1>::Zero();
Matrix<double,3,3> Rbw(q.toRotationMatrix());
A.block(0,0,3,3) = Rbw;
Matrix<double,3,3> W;
skewSymmetric(ang,W);
A.block(3,0,3,3) = -W;
b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
return ;
} /* ----- end of method Body::motionModel ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: skewSymmetric
* Description: Create the skew symmetric matrix y from the vector x.
*--------------------------------------------------------------------------------------
*/
void
Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
{
y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
return ;
} /* ----- end of method Body::skewSymmetric ----- */
|
