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
|
/*
* =====================================================================================
*
* 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"
/*
*--------------------------------------------------------------------------------------
* 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.
*--------------------------------------------------------------------------------------
*/
void
Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
{
h[0] = X[2];
return ;
} /* ----- end of method Body::h_hat ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Body
* Method: Body :: F
* Description: Computes the Jacobian of the motion model of the body.
*--------------------------------------------------------------------------------------
*/
void
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 ;
} /* ----- 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 ----- */
|
