12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Prediction works !

release/4.3a0
Frank Dellaert 2015-12-28 10:00:53 -08:00
parent 68b6d31494
commit 9e99f88473
2 changed files with 30 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/navigation/ScenarioRunner.cpp
View File

@ -35,12 +35,21 @@ using symbol_shorthand::V;
static const Symbol kBiasKey('B', 0); static const Symbol kBiasKey('B', 0);
Vector9 PreintegratedMeasurements2::currentEstimate() const {
VectorValues biasValues;
biasValues.insert(kBiasKey, estimatedBias_.vector());
VectorValues zetaValues = posterior_k_->solve(biasValues);
Vector9 zeta;
zeta << zetaValues.at(T(k_)), Vector3::Zero(), Vector3::Zero();
return zeta;
}
void PreintegratedMeasurements2::integrateMeasurement( void PreintegratedMeasurements2::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
typedef map<Key, Matrix> Terms; typedef map<Key, Matrix> Terms;
static const Matrix36 omega_D_bias = (Matrix36() << Z_3x3, I_3x3).finished(); static const Matrix36 omega_D_bias = (Matrix36() << Z_3x3, I_3x3).finished();
// Correct measurements by subtractig bias // Correct measurements by subtracting bias
Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
GaussianFactorGraph graph; GaussianFactorGraph graph;
@ -49,10 +58,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
// Handle first time differently // Handle first time differently
if (k_ == 0) { if (k_ == 0) {
// theta(1) = measuredOmega - (bias + bias_delta) // theta(1) = (measuredOmega - (bias + bias_delta)) * dt
graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_D_bias}}, graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_D_bias}},
correctedOmega, gyroscopeNoiseModel_); dt * correctedOmega, gyroscopeNoiseModel_);
GTSAM_PRINT(graph);
// eliminate all but biases // eliminate all but biases
Ordering keys = list_of(T(k_ + 1)); Ordering keys = list_of(T(k_ + 1));
@ -60,14 +68,14 @@ void PreintegratedMeasurements2::integrateMeasurement(
graph.eliminatePartialSequential(keys, EliminateQR); graph.eliminatePartialSequential(keys, EliminateQR);
} else { } else {
// add previous posterior // add previous posterior
graph.add(posterior_k_); graph.add(boost::static_pointer_cast<GaussianFactor>(posterior_k_));
// theta(k+1) = theta(k) + inverse(H)*(measuredOmega - (bias + bias_delta)) // theta(k+1) = theta(k) + inverse(H)*(measuredOmega - bias - bias_delta) dt
// => H*theta(k+1) - H*theta(k) + bias_delta = measuredOmega - bias // => H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt
Matrix3 H = Rot3::ExpmapDerivative(theta_); Matrix3 H = Rot3::ExpmapDerivative(theta_);
graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias}}, graph.add<Terms>(
correctedOmega, gyroscopeNoiseModel_); {{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias * dt}},
GTSAM_PRINT(graph); dt * correctedOmega, gyroscopeNoiseModel_);
// eliminate all but biases // eliminate all but biases
Ordering keys = list_of(T(k_))(T(k_ + 1)); Ordering keys = list_of(T(k_))(T(k_ + 1));
@ -75,9 +83,6 @@ void PreintegratedMeasurements2::integrateMeasurement(
graph.eliminatePartialSequential(keys, EliminateQR); graph.eliminatePartialSequential(keys, EliminateQR);
} }
GTSAM_PRINT(*bayesNet);
GTSAM_PRINT(*shouldBeEmpty);
// The bayesNet now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias) // The bayesNet now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias)
// We marginalize zeta(k) by dropping the first factor // We marginalize zeta(k) by dropping the first factor
posterior_k_ = bayesNet->back(); posterior_k_ = bayesNet->back();
@ -87,7 +92,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
NavState PreintegratedMeasurements2::predict( NavState PreintegratedMeasurements2::predict(
const NavState& state_i, const imuBias::ConstantBias& bias_i, const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
return NavState(); // TODO(frank): handle bias
Vector9 zeta = currentEstimate();
return state_i.expmap(zeta);
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////

10
gtsam/navigation/ScenarioRunner.h
View File

@ -41,9 +41,12 @@ class PreintegratedMeasurements2 {
private: private:
SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
const imuBias::ConstantBias estimatedBias_; const imuBias::ConstantBias estimatedBias_;
size_t k_; ///< index/count of measurements integrated size_t k_; ///< index/count of measurements integrated
Vector3 theta_; ///< current estimate for theta Vector3 theta_; ///< current estimate for theta
GaussianFactor::shared_ptr posterior_k_; ///< posterior on current iterate
/// posterior on current iterate, as a conditional P(zeta|bias_delta):
boost::shared_ptr<GaussianConditional> posterior_k_;
public: public:
typedef ImuFactor::PreintegratedMeasurements::Params Params; typedef ImuFactor::PreintegratedMeasurements::Params Params;
@ -71,6 +74,11 @@ class PreintegratedMeasurements2 {
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 6> H2 = boost::none) const; OptionalJacobian<9, 6> H2 = boost::none) const;
private:
// estimate zeta given estimated biases
// calculates conditional mean of P(zeta|bias_delta)
Vector9 currentEstimate() const;
}; };
/* /*