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
|
/***********************************************/
/**
* @file simulateAccelerometerCoMOffset.cpp
*
* @brief Create accelerations due to CoM offset.
*
* @author Beate Klinger
* @date 2016-05-12
*
*/
/***********************************************/
// Latex documentation
#define DOCSTRING docstring
static const char *docstring = R"(
This program generates an \file{accelerometer file}{instrument} containing perturbing accelerations
due to a given center of mass (CoM) offset. This includes centrifugal effects,
Euler forces and the effect of gravity gradients.
)";
/***********************************************/
#include "programs/program.h"
#include "base/polynomial.h"
#include "files/fileInstrument.h"
#include "classes/earthRotation/earthRotation.h"
#include "classes/gravityfield/gravityfield.h"
/***** CLASS ***********************************/
/** @brief Create accelerations due to CoM offset.
* @ingroup programsGroup */
class SimulateAccelerometerCoMOffset
{
public:
void run(Config &config, Parallel::CommunicatorPtr comm);
};
GROOPS_REGISTER_PROGRAM(SimulateAccelerometerCoMOffset, PARALLEL, "Create accelerations due to CoM offset.", Simulation, Instrument)
/***********************************************/
void SimulateAccelerometerCoMOffset::run(Config &config, Parallel::CommunicatorPtr comm)
{
try
{
FileName fileNameOut;
FileName fileNameOrbit, fileNameStarCamera;
Bool applyAngularRate, applyAngularAcc;
GravityfieldPtr gradientfield;
EarthRotationPtr earthRotation;
UInt derivationDegree;
Vector offset(3);
readConfig(config, "outputfileAccelerometer", fileNameOut, Config::MUSTSET, "", "effect of offset");
readConfig(config, "inputfileOrbit", fileNameOrbit, Config::MUSTSET, "", "");
readConfig(config, "inputfileStarCamera", fileNameStarCamera, Config::MUSTSET, "", "");
readConfig(config, "applyAngularRate", applyAngularRate, Config::DEFAULT, "1", "compute effect of centrifugal forces");
readConfig(config, "applyAngularAccelerations", applyAngularAcc, Config::DEFAULT, "1", "compute effect of Euler forces");
readConfig(config, "gradientfield", gradientfield, Config::DEFAULT, "", "low order field to estimate the change of the gravity by position adjustement");
readConfig(config, "earthRotation", earthRotation, Config::MUSTSET, "", "");
readConfig(config, "interpolationDegree", derivationDegree, Config::DEFAULT, "2", "derivation of quaternions by polynomial interpolation of degree n");
readConfig(config, "CoMOffsetX", offset(0), Config::MUSTSET, "50e-6", "offset [m]");
readConfig(config, "CoMOffsetY", offset(1), Config::MUSTSET, "50e-6", "offset [m]");
readConfig(config, "CoMOffsetZ", offset(2), Config::MUSTSET, "50e-6", "offset [m]");
if(isCreateSchema(config)) return;
logStatus<<"compute accelerations (CoM)"<<Log::endl;
InstrumentFile orbitFile(fileNameOrbit);
InstrumentFile starCameraFile(fileNameStarCamera);
InstrumentFile::checkArcCount({orbitFile, starCameraFile});
std::vector<Arc> arcList(orbitFile.arcCount());
Parallel::forEach(arcList, [&](UInt arcNo)
{
OrbitArc orbit = orbitFile.readArc(arcNo);
StarCameraArc starCamera = starCameraFile.readArc(arcNo);
Arc::checkSynchronized({orbit, starCamera});
// quaternions
const std::vector<Time> times = orbit.times();
Polynomial polynomial(times, derivationDegree);
Matrix q = starCamera.matrix().column(1,4);
Matrix dq = polynomial.derivative(times, q);
Matrix ddq = polynomial.derivative2nd(times, q);
AccelerometerArc accelerometerArc;
for(UInt i=0; i<orbit.size(); i++)
{
// Gravity gradient in satellite system (SRF)
Rotary3d rotEarth = earthRotation->rotaryMatrix(orbit.at(i).time);
Tensor3d T = gradientfield->gravityGradient(orbit.at(i).time, rotEarth.rotate(orbit.at(i).position));
T = starCamera.at(i).rotary.inverseRotate(rotEarth.inverseRotate(T));
// angular rates/accelerations
Vector w(3), wd(3);
Matrix Q(3, 4);
Q(0,0) = -q(i,1); Q(0,1) = +q(i,0); Q(0,2) = +q(i,3); Q(0,3) = -q(i,2);
Q(1,1) = -q(i,2); Q(1,1) = -q(i,3); Q(1,2) = +q(i,0); Q(1,3) = +q(i,1);
Q(2,2) = -q(i,3); Q(2,1) = +q(i,2); Q(2,2) = -q(i,1); Q(2,3) = +q(i,0);
if(applyAngularRate)
w = 2*Q * dq.row(i).trans();
if(applyAngularAcc)
wd = 2*Q * ddq.row(i).trans(); // angular accelerations
// Center of mass offset
Matrix M = -T.matrix();
M(0,0) += -w(1)*w(1)-w(2)*w(2); M(0,1) += w(0)*w(1)-wd(2); M(0,2) += w(0)*w(2)+wd(1);
M(1,0) += w(0)*w(1)+wd(2); M(1,1) += -w(0)*w(0)-w(2)*w(2); M(1,2) += w(1)*w(2)-wd(0);
M(2,0) += w(0)*w(2)-wd(1); M(2,1) += w(1)*w(2)+wd(0); M(2,2) += -w(0)*w(0)-w(1)*w(1);
Vector a = M * offset;
AccelerometerEpoch epoch;
epoch.time = orbit.at(i).time;
epoch.acceleration.x() = a(0);
epoch.acceleration.y() = a(1);
epoch.acceleration.z() = a(2);
accelerometerArc.push_back(epoch);
}
return accelerometerArc;
}, comm);
if(Parallel::isMaster(comm))
{
logStatus<<"write data to file <"<<fileNameOut<<">"<<Log::endl;
InstrumentFile::write(fileNameOut, arcList);
}
}
catch(std::exception &e)
{
GROOPS_RETHROW(e)
}
}
/***********************************************/
|
