inertialMeasurementSim.cpp
Go to the documentation of this file.
1 
7 #include "inertialMeasurementSim.hpp"
8 #include <chrono>
9 
18 inertialMeasurementSim::inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance,
19  double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation){
20 
21  // RNG seed from current time
22  randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count());
23 
24  accMeasNoiseVariance_ = accMeasNoiseVariance;
25  gyroMeasNoiseVariance_ = gyroMeasNoiseVariance;
26  accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
27  gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
28 }
29 
38 void inertialMeasurementSim::setBias(const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias,
39  double accBiasProcessNoiseAutoCorrelation,
40  double gyroBiasProcessNoiseAutoCorrelation){
41  accBias_ = accBias;
42  gyroBias_ = gyroBias;
43  accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
44  gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
45 }
46 
55 void inertialMeasurementSim::setBias(double accBiasVariance, double gyroBiasVariance,
56  double accBiasProcessNoiseAutoCorrelation,
57  double gyroBiasProcessNoiseAutoCorrelation){
58 
59  accBias_ << sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
60  sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
61  sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
62 
63  gyroBias_ << sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
64  sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
65  sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
66 
67  accBiasProcessNoiseAutoCorrelation_ = accBiasProcessNoiseAutoCorrelation;
68  gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation;
69 }
70 
77 void inertialMeasurementSim::setBias(double accBiasVariance, double gyroBiasVariance){
78 
79  accBias_ << sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
80  sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
81  sqrt(accBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
82 
83  gyroBias_ << sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
84  sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_),
85  sqrt(gyroBiasVariance)*standardNormalDistribution_(randomNumberGenerator_);
86 }
87 
94 void inertialMeasurementSim::setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance){
95  accMeasNoiseVariance_ = accMeasNoiseVariance;
96  gyroMeasNoiseVariance_ = gyroMeasNoiseVariance;
97 }
98 
104 void inertialMeasurementSim::setOrientation(const Eigen::Quaterniond & imuOrient){
105  imuOrient_ = imuOrient;
106 }
107 
116 void inertialMeasurementSim::getMeasurement(Eigen::Vector3d & accOutput, Eigen::Vector3d & gyroOutput,
117  const Eigen::Vector3d specificForce, const Eigen::Vector3d angularVelocity){
118 
119  Eigen::Vector3d accNoise;
120 
121  accNoise << sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
122  sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
123  sqrt(accMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_);
124 
125  Eigen::Vector3d gyroNoise;
126 
127  gyroNoise << sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
128  sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_),
129  sqrt(gyroMeasNoiseVariance_)*standardNormalDistribution_(randomNumberGenerator_);
130 
131  accOutput = imuOrient_.inverse()*specificForce + accBias_ + accNoise;
132  gyroOutput = imuOrient_.inverse()*angularVelocity + gyroBias_ + gyroNoise;
133 }
134 
140 void inertialMeasurementSim::proceedBiasDynamics(double dt_secs){
141  Eigen::Vector3d accBiasDerivative;
142 
143  accBiasDerivative << sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
144  sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
145  sqrt(accBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_);
146 
147  Eigen::Vector3d gyroBiasDerivative;
148 
149  gyroBiasDerivative << sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
150  sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_),
151  sqrt(gyroBiasProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_);
152 
153  accBias_ += dt_secs*accBiasDerivative;
154  gyroBias_ += dt_secs*gyroBiasDerivative;
155 }
inertialMeasurementSim::setNoiseVariance
void setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance)
Set additive noise variances.
Definition: inertialMeasurementSim.cpp:94
inertialMeasurementSim::gyroBiasProcessNoiseAutoCorrelation_
double gyroBiasProcessNoiseAutoCorrelation_
Definition: inertialMeasurementSim.hpp:59
inertialMeasurementSim::imuOrient_
Eigen::Quaterniond imuOrient_
Definition: inertialMeasurementSim.hpp:70
inertialMeasurementSim::accBiasProcessNoiseAutoCorrelation_
double accBiasProcessNoiseAutoCorrelation_
Definition: inertialMeasurementSim.hpp:58
inertialMeasurementSim::setBias
void setBias(const Eigen::Vector3d &accBias, const Eigen::Vector3d &gyroBias, double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation)
Set bias properties.
Definition: inertialMeasurementSim.cpp:38
inertialMeasurementSim::accMeasNoiseVariance_
double accMeasNoiseVariance_
Definition: inertialMeasurementSim.hpp:52
inertialMeasurementSim.hpp
Inertial measurement unit (IMU) simulator class header file.
inertialMeasurementSim::standardNormalDistribution_
std::normal_distribution< double > standardNormalDistribution_
Definition: inertialMeasurementSim.hpp:47
inertialMeasurementSim::gyroBias_
Eigen::Vector3d gyroBias_
Definition: inertialMeasurementSim.hpp:65
inertialMeasurementSim::getMeasurement
void getMeasurement(Eigen::Vector3d &accOutput, Eigen::Vector3d &gyroOutput, const Eigen::Vector3d specificForce, const Eigen::Vector3d angularVelocity)
Get IMU measurement.
Definition: inertialMeasurementSim.cpp:116
inertialMeasurementSim::randomNumberGenerator_
std::default_random_engine randomNumberGenerator_
Definition: inertialMeasurementSim.hpp:46
inertialMeasurementSim::setOrientation
void setOrientation(const Eigen::Quaterniond &imuOrient)
Set IMU orientation with regard to body-frame.
Definition: inertialMeasurementSim.cpp:104
inertialMeasurementSim::inertialMeasurementSim
inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance, double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation)
Construct a new IMU Sim object.
Definition: inertialMeasurementSim.cpp:18
time_since_epoch
double time_since_epoch()
inertialMeasurementSim::gyroMeasNoiseVariance_
double gyroMeasNoiseVariance_
Definition: inertialMeasurementSim.hpp:53
inertialMeasurementSim::accBias_
Eigen::Vector3d accBias_
Definition: inertialMeasurementSim.hpp:64
inertialMeasurementSim::proceedBiasDynamics
void proceedBiasDynamics(double dt_secs)
Proceed accelerometer and gyroscope bias dynamics.
Definition: inertialMeasurementSim.cpp:140

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Mon Dec 9 2024 03:13:35