27static noiseModel::Diagonal::shared_ptr Diagonal(
const Matrix& covariance) {
30 auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
32 throw std::invalid_argument(
"ScenarioRunner::Diagonal: not a diagonal");
43 typedef boost::shared_ptr<PreintegrationParams> SharedParams;
47 const SharedParams p_;
48 const double imuSampleTime_, sqrt_dt_;
49 const Bias estimatedBias_;
52 Sampler gyroSampler_, accSampler_;
56 double imuSampleTime = 1.0 / 100.0,
const Bias& bias =
Bias())
57 : scenario_(scenario),
59 imuSampleTime_(imuSampleTime),
60 sqrt_dt_(std::sqrt(imuSampleTime)),
63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
68 const Vector3& gravity_n()
const {
return p_->n_gravity; }
70 const Scenario& scenario()
const {
return scenario_; }
73 Vector3 actualAngularVelocity(
double t)
const {
return scenario_.
omega_b(t); }
76 Vector3 actualSpecificForce(
double t)
const {
78 return scenario_.acceleration_b(t) - bRn * gravity_n();
82 Vector3 measuredAngularVelocity(
double t)
const {
83 return actualAngularVelocity(t) + estimatedBias_.
gyroscope() +
84 gyroSampler_.
sample() / sqrt_dt_;
86 Vector3 measuredSpecificForce(
double t)
const {
87 return actualSpecificForce(t) + estimatedBias_.
accelerometer() +
88 accSampler_.
sample() / sqrt_dt_;
91 const double& imuSampleTime()
const {
return imuSampleTime_; }
96 bool corrupted =
false)
const;
100 const Bias& estimatedBias =
Bias())
const;
103 Matrix9 estimateCovariance(
double T,
size_t N = 1000,
104 const Bias& estimatedBias =
Bias())
const;
107 Matrix6 estimateNoiseCovariance(
size_t N = 1000)
const;
116 typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
119 const SharedParams p_;
120 const Bias estimatedBias_;
124 double imuSampleTime = 1.0 / 100.0,
126 :
ScenarioRunner(scenario,
static_cast<ScenarioRunner::SharedParams
>(p),
127 imuSampleTime, bias),
129 estimatedBias_(bias) {}
133 double T,
const Bias& estimatedBias =
Bias(),
134 bool corrupted =
false)
const;
138 const Bias& estimatedBias =
Bias())
const;
141 Eigen::Matrix<double, 15, 15> estimateCovariance(
142 double T,
size_t N = 1000,
const Bias& estimatedBias =
Bias())
const;
sampling from a NoiseModel
Simple class to test navigation scenarios.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition: Rot3M.cpp:144
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:117
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition: Sampler.h:31
Vector sample() const
sample from distribution
Definition: Sampler.cpp:59
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition: CombinedImuFactor.h:129
const Vector3 & gyroscope() const
get gyroscope bias
Definition: ImuBias.h:69
const Vector3 & accelerometer() const
get accelerometer bias
Definition: ImuBias.h:64
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition: ImuFactor.h:72
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
Simple trajectory simulator.
Definition: Scenario.h:25
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
Definition: ScenarioRunner.h:40
Definition: ScenarioRunner.h:114