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
|
/***********************************************/
/**
* @file instrumentStarCamera2AccAngularRate.cpp
*
* @brief Derivation of angular rates from rotations.
*
* @author Torsten Mayer-Guerr
* @date 2011-04-18
*/
/***********************************************/
// Latex documentation
#define DOCSTRING docstring
static const char *docstring = R"(
This program derivate from a time series of quaternions
a series of angular rates and angular accelerations.
The derivatives are computed by a polynomial interpolation
with \config{interpolationDegree} of the quaternions.
)";
/***********************************************/
#include "programs/program.h"
#include "base/polynomial.h"
#include "files/fileInstrument.h"
/***** CLASS ***********************************/
/** @brief Derivation of angular rates from rotations.
* @ingroup programsGroup */
class InstrumentStarCamera2AccAngularRate
{
public:
void run(Config &config, Parallel::CommunicatorPtr comm);
};
GROOPS_REGISTER_PROGRAM(InstrumentStarCamera2AccAngularRate, SINGLEPROCESS, "derivation of angular rates from rotations", Instrument)
/***********************************************/
void InstrumentStarCamera2AccAngularRate::run(Config &config, Parallel::CommunicatorPtr /*comm*/)
{
try
{
FileName angRateName, angAccName;
FileName starCameraName;
UInt derivationDegree;
readConfig(config, "outputfileAngularRate", angRateName, Config::OPTIONAL, "", "[rad/s], VECTOR3D");
readConfig(config, "outputfileAngularAcc", angAccName, Config::OPTIONAL, "", "[rad/s**2], VECTOR3D");
readConfig(config, "inputfileStarCamera", starCameraName, Config::MUSTSET, "", "");
readConfig(config, "interpolationDegree", derivationDegree, Config::DEFAULT, "2", "derivation by polynomial interpolation of degree n");
if(isCreateSchema(config)) return;
// init interpolation coefficients
// -------------------------------
// calculate angular rates & accelerations
// ---------------------------------------
logStatus<<"calculate angular rates & accelerations"<<Log::endl;
InstrumentFile starCameraFile(starCameraName);
std::list<Arc> angRateArcList, angAccArcList;
Single::forEach(starCameraFile.arcCount(), [&](UInt arcNo)
{
const StarCameraArc scaArc = starCameraFile.readArc(arcNo);
const std::vector<Time> times = scaArc.times();
const Matrix q = scaArc.matrix().column(1,4);
Polynomial polynomial(times, derivationDegree);
// angular rate
// ------------
Matrix dq = polynomial.derivative(times, q);
Vector3dArc angRateArc;
for(UInt i=0; i<scaArc.size(); i++)
{
Vector3dEpoch angRate;
angRate.time = scaArc.at(i).time;
angRate.vector3d.x() = 2*(-q(i,1)*dq(i,0) + q(i,0)*dq(i,1) + q(i,3)*dq(i,2) - q(i,2)*dq(i,3));
angRate.vector3d.y() = 2*(-q(i,2)*dq(i,0) - q(i,3)*dq(i,1) + q(i,0)*dq(i,2) + q(i,1)*dq(i,3));
angRate.vector3d.z() = 2*(-q(i,3)*dq(i,0) + q(i,2)*dq(i,1) - q(i,1)*dq(i,2) + q(i,0)*dq(i,3));
angRateArc.push_back(angRate);
}
angRateArcList.push_back(angRateArc);
// angular accelerations
// ---------------------
if(!angAccName.empty())
{
Matrix ddq = polynomial.derivative2nd(times, q);
Vector3dArc angAccArc;
for(UInt i=0; i<scaArc.size(); i++)
{
Vector3dEpoch angAcc;
angAcc.time = scaArc.at(i).time;
angAcc.vector3d.x() = 2*(-q(i,1)*ddq(i,0) + q(i,0)*ddq(i,1) + q(i,3)*ddq(i,2) - q(i,2)*ddq(i,3));
angAcc.vector3d.y() = 2*(-q(i,2)*ddq(i,0) - q(i,3)*ddq(i,1) + q(i,0)*ddq(i,2) + q(i,1)*ddq(i,3));
angAcc.vector3d.z() = 2*(-q(i,3)*ddq(i,0) + q(i,2)*ddq(i,1) - q(i,1)*ddq(i,2) + q(i,0)*ddq(i,3));
angAccArc.push_back(angAcc);
}
angAccArcList.push_back(angAccArc);
}
});
// write data
// ----------
if(!angRateName.empty())
{
logStatus<<"write angular rate data to file <"<<angRateName<<">"<<Log::endl;
InstrumentFile::write(angRateName, angRateArcList);
}
if(!angAccName.empty())
{
logStatus<<"write angular accelerations data to file <"<<angAccName<<">"<<Log::endl;
InstrumentFile::write(angAccName, angAccArcList);
}
}
catch(std::exception &e)
{
GROOPS_RETHROW(e)
}
}
/***********************************************/
|
