-
Notifications
You must be signed in to change notification settings - Fork 6
/
inertialMeasurementSim.hpp
78 lines (61 loc) · 2.67 KB
/
inertialMeasurementSim.hpp
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
/**
* @file inertialMeasurementSim.hpp
* @author Ezra Tal
* @brief Inertial measurement unit (IMU) simulator class header file
*
*/
#ifndef INERTIALMEASUREMENTSIM_H
#define INERTIALMEASUREMENTSIM_H
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <random>
/**
* @brief Inertial measurement unit (IMU) simulator class
*
*/
class inertialMeasurementSim
{
public:
inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance,
double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation);
void setRandomSeed(const unsigned imuSeed);
void setBias(const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias,
double accBiasProcessNoiseAutoCorrelation,
double gyroBiasProcessNoiseAutoCorrelation);
void setBias(double accBiasVariance, double gyroBiasVariance,
double accBiasProcessNoiseAutoCorrelation,
double gyroBiasProcessNoiseAutoCorrelation);
void setBias(double accBiasVariance, double gyroBiasVariance);
void getBias(Eigen::Vector3d & accBias, Eigen::Vector3d & gyroBias);
void setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance);
void setOrientation(const Eigen::Quaterniond & imuOrient);
void getMeasurement(Eigen::Vector3d & accOutput, Eigen::Vector3d & gyroOutput,
const Eigen::Vector3d specificForce, const Eigen::Vector3d angularVelocity);
void proceedBiasDynamics(double dt_secs);
private:
/// @name Std normal RNG
//@{
std::default_random_engine randomNumberGenerator_;
std::normal_distribution<double> standardNormalDistribution_ = std::normal_distribution<double>(0.0,1.0);
//@}
/// @name Measurement noise variance
//@{
double accMeasNoiseVariance_ = 0.; // m^2/s^4
double gyroMeasNoiseVariance_ = 0.; // rad^2/s^2
//@}
/// @name Bias dynamics process noise auto correlation
//@{
double accBiasProcessNoiseAutoCorrelation_ = 0.; // m^2/s^5
double gyroBiasProcessNoiseAutoCorrelation_ = 0.; // rad^2/s^3
//@}
/// @name Bias states
//@{
Eigen::Vector3d accBias_ = Eigen::Vector3d::Zero(); // m/s^2
Eigen::Vector3d gyroBias_ = Eigen::Vector3d::Zero(); // rad/s
//@}
/// @name Orientation of IMU wrt body-fixed frame
//@{
Eigen::Quaterniond imuOrient_ = Eigen::Quaterniond::Identity();
//@}
};
#endif // INERTIALMEASUREMENTSIM_H