107 lines
3.0 KiB
C++
107 lines
3.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file ScenarioRunner.h
|
|
* @brief Simple class to test navigation scenarios
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/navigation/ScenarioRunner.h>
|
|
#include <cmath>
|
|
|
|
using namespace std;
|
|
using namespace boost::assign;
|
|
|
|
namespace gtsam {
|
|
|
|
static double intNoiseVar = 0.0000001;
|
|
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
|
|
|
AggregateImuReadings ScenarioRunner::integrate(double T,
|
|
const Bias& estimatedBias,
|
|
bool corrupted) const {
|
|
AggregateImuReadings pim(p_, estimatedBias);
|
|
|
|
const double dt = imuSampleTime();
|
|
const size_t nrSteps = T / dt;
|
|
double t = 0;
|
|
for (size_t k = 0; k < nrSteps; k++, t += dt) {
|
|
Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t);
|
|
Vector3 measuredAcc =
|
|
corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t);
|
|
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
|
}
|
|
|
|
return pim;
|
|
}
|
|
|
|
NavState ScenarioRunner::predict(const AggregateImuReadings& pim,
|
|
const Bias& estimatedBias) const {
|
|
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
|
|
return pim.predict(state_i, estimatedBias);
|
|
}
|
|
|
|
Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
|
|
const Bias& estimatedBias) const {
|
|
// Get predict prediction from ground truth measurements
|
|
NavState prediction = predict(integrate(T));
|
|
|
|
// Sample !
|
|
Matrix samples(9, N);
|
|
Vector9 sum = Vector9::Zero();
|
|
for (size_t i = 0; i < N; i++) {
|
|
auto pim = integrate(T, estimatedBias, true);
|
|
#if 0
|
|
NavState sampled = predict(pim);
|
|
Vector9 zeta = sampled.localCoordinates(prediction);
|
|
#else
|
|
Vector9 xi = pim.zeta();
|
|
#endif
|
|
samples.col(i) = xi;
|
|
sum += xi;
|
|
}
|
|
|
|
// Compute MC covariance
|
|
Vector9 sampleMean = sum / N;
|
|
Matrix9 Q;
|
|
Q.setZero();
|
|
for (size_t i = 0; i < N; i++) {
|
|
Vector9 xi = samples.col(i) - sampleMean;
|
|
Q += xi * xi.transpose();
|
|
}
|
|
|
|
return Q / (N - 1);
|
|
}
|
|
|
|
Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
|
|
Matrix samples(6, N);
|
|
Vector6 sum = Vector6::Zero();
|
|
for (size_t i = 0; i < N; i++) {
|
|
samples.col(i) << accSampler_.sample() / sqrt_dt_,
|
|
gyroSampler_.sample() / sqrt_dt_;
|
|
sum += samples.col(i);
|
|
}
|
|
|
|
// Compute MC covariance
|
|
Vector6 sampleMean = sum / N;
|
|
Matrix6 Q;
|
|
Q.setZero();
|
|
for (size_t i = 0; i < N; i++) {
|
|
Vector6 xi = samples.col(i) - sampleMean;
|
|
Q += xi * xi.transpose();
|
|
}
|
|
|
|
return Q / (N - 1);
|
|
}
|
|
|
|
} // namespace gtsam
|