-
Notifications
You must be signed in to change notification settings - Fork 6
/
inertialMeasurementSim.cpp
175 lines (149 loc) · 7.4 KB
/
inertialMeasurementSim.cpp
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
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
/**
* @file inertialMeasurementSim.cpp
* @author Ezra Tal
* @brief Inertial measurement unit (IMU) simulator class implementation
*
*/
#include "inertialMeasurementSim.hpp"
#include <chrono>
/**
* @brief Construct a new IMU Sim object
*
* @param accMeasNoiseVariance Accelerometer additive noise variance
* @param gyroMeasNoiseVariance Gyroscope additive noise variance
* @param accBiasProcessNoiseAutoCorrelation Accelerometer bias process noise auto correlation
* @param gyroBiasProcessNoiseAutoCorrelation Gyroscope bias process noise auto correlation
*/
inertialMeasurementSim::inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance,
double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation){
// RNG seed from current time
randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count());
accMeasNoiseVariance_ = accMeasNoiseVariance;
gyroMeasNoiseVariance_ = gyroMeasNoiseVariance;
accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
}
/**
* @brief Set seed of random number generator
*
* @param imuSeed Seed for IMU measurement noise and bias dynamics RNG.
*/
void inertialMeasurementSim::setRandomSeed(const unsigned imuSeed){
randomNumberGenerator_.seed(imuSeed);
}
/**
* @brief Set bias properties
*
* @param accBias Accelerometer bias value
* @param gyroBias Gyroscope bias value
* @param accBiasProcessNoiseAutoCorrelation Accelerometer bias process noise auto correlation
* @param gyroBiasProcessNoiseAutoCorrelation Gyroscope bias process noise auto correlation
*/
void inertialMeasurementSim::setBias(const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias,
double accBiasProcessNoiseAutoCorrelation,
double gyroBiasProcessNoiseAutoCorrelation){
accBias_ = accBias;
gyroBias_ = gyroBias;
accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
}
/**
* @brief Get bias values
*
* @param accBias Accelerometer bias value
* @param gyroBias Gyroscope bias value
*/
void inertialMeasurementSim::getBias(Eigen::Vector3d & accBias, Eigen::Vector3d & gyroBias){
accBias = accBias_;
gyroBias = gyroBias_;
}
/**
* @brief Set bias properties
*
* @param accBias Accelerometer bias variance
* @param gyroBias Gyroscope bias variance
* @param accBiasProcessNoiseAutoCorrelation Accelerometer bias process noise auto correlation
* @param gyroBiasProcessNoiseAutoCorrelation Gyroscope bias process noise auto correlation
*/
void inertialMeasurementSim::setBias(double accBiasVariance, double gyroBiasVariance,
double accBiasProcessNoiseAutoCorrelation,
double gyroBiasProcessNoiseAutoCorrelation){
accBias_ << sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
gyroBias_ << sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
}
/**
* @brief Set bias
*
* @param accBias Accelerometer bias variance
* @param gyroBias Gyroscope bias variance
*/
void inertialMeasurementSim::setBias(double accBiasVariance, double gyroBiasVariance){
accBias_ << sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
gyroBias_ << sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
}
/**
* @brief Set additive noise variances
*
* @param accMeasNoiseVariance Accelerometer additive noise variance
* @param gyroMeasNoiseVariance Gyroscope additive noise variance
*/
void inertialMeasurementSim::setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance){
accMeasNoiseVariance_ = accMeasNoiseVariance;
gyroMeasNoiseVariance_ = gyroMeasNoiseVariance;
}
/**
* @brief Set IMU orientation with regard to body-frame
*
* @param imuOrient IMU orientation with regard to body-frame
*/
void inertialMeasurementSim::setOrientation(const Eigen::Quaterniond & imuOrient){
imuOrient_ = imuOrient;
}
/**
* @brief Get IMU measurement
*
* @param accOutput Accelerometer output in IMU frame
* @param gyroOutput Gyroscope output in IMU frame
* @param specificForce Specific force in body-frame
* @param angularVelocity Angular velocity in body-frame
*/
void inertialMeasurementSim::getMeasurement(Eigen::Vector3d & accOutput, Eigen::Vector3d & gyroOutput,
const Eigen::Vector3d specificForce, const Eigen::Vector3d angularVelocity){
Eigen::Vector3d accNoise;
accNoise << sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_);
Eigen::Vector3d gyroNoise;
gyroNoise << sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_);
accOutput = imuOrient_.inverse()*specificForce + accBias_ + accNoise;
gyroOutput = imuOrient_.inverse()*angularVelocity + gyroBias_ + gyroNoise;
}
/**
* @brief Proceed accelerometer and gyroscope bias dynamics
*
* @param dt_secs Time step
*/
void inertialMeasurementSim::proceedBiasDynamics(double dt_secs){
Eigen::Vector3d accBiasDerivative;
accBiasDerivative << sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_);
Eigen::Vector3d gyroBiasDerivative;
gyroBiasDerivative << sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_);
accBias_ += dt_secs*accBiasDerivative;
gyroBias_ += dt_secs*gyroBiasDerivative;
}