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
139
140
141
142
143
144 | /**
* @file InertiaSensorCalibrator.h
*
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* @author <a href="mailto:mellmann@informatik.hu-berlin.de">Heinrich Mellmann</a>
* @author Inspired by the BH-2011
*
* Calibrating the sensors: Inertial, Accelerometer, Gyrometer.
*/
#ifndef _InertiaSensorCalibrator_h_
#define _InertiaSensorCalibrator_h_
#include <ModuleFramework/Module.h>
#include "Tools/Math/Kalman.h"
#include "Tools/DataStructures/RingBuffer.h"
#include "Tools/DataStructures/RingBufferWithSum.h"
#include "Tools/Debug/NaoTHAssert.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugPlot.h"
#include "Tools/Debug/DebugParameterList.h"
// representations
#include <Representations/Infrastructure/FrameInfo.h>
#include <Representations/Infrastructure/AccelerometerData.h>
#include <Representations/Infrastructure/GyrometerData.h>
#include <Representations/Infrastructure/InertialSensorData.h>
#include <Representations/Infrastructure/JointData.h>
#include "Representations/Infrastructure/CalibrationData.h"
#include "Representations/Modeling/GroundContactModel.h"
#include "Representations/Motion/MotionStatus.h"
#include "Representations/Modeling/KinematicChain.h"
BEGIN_DECLARE_MODULE(InertiaSensorCalibrator)
PROVIDE(DebugRequest)
PROVIDE(DebugModify)
PROVIDE(DebugPlot)
PROVIDE(DebugParameterList)
REQUIRE(AccelerometerData)
REQUIRE(GyrometerData)
REQUIRE(InertialSensorData)
REQUIRE(FrameInfo)
REQUIRE(MotorJointData)
REQUIRE(KinematicChainSensor)
REQUIRE(GroundContactModel)
REQUIRE(MotionStatus)
PROVIDE(CalibrationData)
END_DECLARE_MODULE(InertiaSensorCalibrator)
class InertiaSensorCalibrator: private InertiaSensorCalibratorBase
{
public:
InertiaSensorCalibrator();
~InertiaSensorCalibrator();
void execute();
private:
bool intentionallyMoving();
void reset();
class Parameter : public ParameterList
{
public:
Parameter() : ParameterList("InertiaSensorCalibrator")
{
PARAMETER_REGISTER(disable) = false;
syncWithConfig();
}
bool disable;
} parameter;
private:
// inertial
Kalman<double> inertialXBias; /**< The calibration bias of inertialX. */
Kalman<double> inertialYBias; /**< The calibration bias of inertialY. */
// accelerometer
Kalman<double> accXBias; /**< The calibration bias of accX. */
Kalman<double> accYBias; /**< The calibration bias of accY. */
Kalman<double> accZBias; /**< The calibration bias of accZ. */
// gyro
Kalman<double> gyroXBias; /**< The calibration bias of gyroX. */
Kalman<double> gyroYBias; /**< The calibration bias of gyroY. */
Kalman<double> gyroZBias; /**< The calibration bias of gyroZ. */
bool calibrated; /**< Whether the filters are initialized. */
unsigned int lastTime; /**< The time of the previous iteration. */
unsigned int collectionStartTime; /**< When the current collection was started. */
unsigned int stableStartTime; /**< When the last unstable situation was over. */
RingBufferWithSum<Vector2d, 300> inertialValues; /**< Ringbuffer for collecting the acceleration sensor values of one walking phase or 1 sec. */
RingBufferWithSum<Vector3d, 300> accValues; /**< Ringbuffer for collecting the acceleration sensor values of one walking phase or 1 sec. */
RingBufferWithSum<Vector3d, 300> gyroValues; /**< Ringbuffer for collecting the gyro sensor values of one walking phase or 1 sec. */
/**
* Class for buffering averaged gyro and acceleration sensor readings.
*/
class Collection
{
public:
Vector2d inertialAvg; /**< The average of acceleration sensor readings of one walking phase or 1 sec. */
Vector3d accAvg; /**< The average of acceleration sensor readings of one walking phase or 1 sec. */
Vector3d gyroAvg; /**< The average of gyro sensor eadings of one walking phase or 1 sec. */
unsigned int timeStamp; /**< When this collection was created. */
/**
* Constructs a collection.
*/
Collection(
const Vector2d& inertialAvg,
const Vector3d& accAvg,
const Vector3d& gyroAvg,
unsigned int timeStamp)
:
inertialAvg(inertialAvg),
accAvg(accAvg),
gyroAvg(gyroAvg),
timeStamp(timeStamp){}
/**
* Default constructor.
*/
Collection() {}<--- Member variable 'Collection::timeStamp' is not initialized in the constructor.
};
RingBuffer<Collection, 50> collections; /**< Buffered averaged gyro and accleration sensor readings. */
};
#endif // _InertiaSensorCalibrator_h_
|