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
|
/*
* =====================================================================================
*
* 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"
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 ----- */
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 ----- */
|
