discrete noise models

release/4.3a0
Frank Dellaert 2015-12-29 09:47:56 -08:00
parent 0dfd44f26c
commit e52f7ec705
2 changed files with 27 additions and 11 deletions

View File

@ -54,29 +54,43 @@ Vector3 PreintegratedMeasurements2::currentTheta() const {
return zetaValues.at(T(k_));
}
SharedDiagonal PreintegratedMeasurements2::discreteAccelerometerNoiseModel(
double dt) const {
return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() /
std::sqrt(dt));
}
SharedDiagonal PreintegratedMeasurements2::discreteGyroscopeNoiseModel(
double dt) const {
return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() /
std::sqrt(dt));
}
PreintegratedMeasurements2::SharedBayesNet
PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc,
const Vector3& correctedOmega,
double dt) const {
typedef map<Key, Matrix> Terms;
// We create a factor graph and then compute P(zeta|bias)
GaussianFactorGraph graph;
// theta(1) = (correctedOmega - bias_delta) * dt
// => theta(1) + bias_delta * dt = correctedOmega * dt
graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_H_bias * dt}},
correctedOmega * dt, gyroscopeNoiseModel_);
correctedOmega * dt, discreteGyroscopeNoiseModel(dt));
// pose(1) = (correctedAcc - bias_delta) * dt^2/2
// => pose(1) + bias_delta * dt^2/2 = correctedAcc * dt^2/2
double dt22 = 0.5 * dt * dt;
auto accModel = discreteAccelerometerNoiseModel(dt);
graph.add<Terms>({{P(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt22}},
correctedAcc * dt22, accelerometerNoiseModel_);
correctedAcc * dt22, accModel);
// vel(1) = (correctedAcc - bias_delta) * dt
// => vel(1) + bias_delta * dt = correctedAcc * dt
graph.add<Terms>({{V(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt}},
correctedAcc * dt, accelerometerNoiseModel_);
correctedAcc * dt, accModel);
// eliminate all but biases
// NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias)
@ -105,23 +119,24 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc,
// => H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt
Matrix3 H = Rot3::ExpmapDerivative(theta_k);
graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias * dt}},
correctedOmega * dt, gyroscopeNoiseModel_);
correctedOmega * dt, discreteGyroscopeNoiseModel(dt));
// pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt^2/2
// => Rkt*pos(k+1) - Rkt*pos(k) - Rkt*vel(k) dt + bias_delta dt^2/2
// = correctedAcc dt^2/2
double dt22 = 0.5 * dt * dt;
auto accModel = discreteAccelerometerNoiseModel(dt);
graph.add<Terms>({{P(k_ + 1), Rkt},
{P(k_), -Rkt},
{V(k_), -Rkt * dt},
{kBiasKey, acc_H_bias * dt22}},
correctedAcc * dt22, accelerometerNoiseModel_);
correctedAcc * dt22, accModel);
// vel(k+1) = vel(k) + Rk*(correctedAcc - bias_delta) dt
// => Rkt*vel(k+1) - Rkt*vel(k) + bias_delta dt = correctedAcc * dt
graph.add<Terms>(
{{V(k_ + 1), Rkt}, {V(k_), -Rkt}, {kBiasKey, acc_H_bias * dt}},
correctedAcc * dt, accelerometerNoiseModel_);
correctedAcc * dt, accModel);
// eliminate all but biases
// TODO(frank): does not seem to eliminate in order I want. What gives?
@ -182,11 +197,7 @@ NavState PreintegratedMeasurements2::predict(
SharedGaussian PreintegratedMeasurements2::noiseModel() const {
Matrix RS;
Vector d;
GTSAM_PRINT(*posterior_k_);
boost::tie(RS, d) = posterior_k_->matrix();
cout << RS << endl
<< endl;
cout << d.transpose() << endl;
// R'*R = A'*A = inv(Cov)
// TODO(frank): think of a faster way - implement in noiseModel

View File

@ -94,6 +94,12 @@ class PreintegratedMeasurements2 {
// estimate theta given estimated biases
Vector3 currentTheta() const;
// We obtain discrete-time noise models by dividing the continuous-time
// covariances by dt:
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
// initialize posterior with first (corrected) IMU measurement
SharedBayesNet initPosterior(const Vector3& correctedAcc,
const Vector3& correctedOmega, double dt) const;
@ -102,7 +108,6 @@ class PreintegratedMeasurements2 {
SharedBayesNet integrateCorrected(const Vector3& correctedAcc,
const Vector3& correctedOmega,
double dt) const;
};
/*