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

Correct for gravity and V0

release/4.3a0
Frank Dellaert 2015-12-28 13:17:41 -08:00
parent 06b1f381ea
commit d3d3b8399d
3 changed files with 133 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/navigation/ScenarioRunner.cpp
View File

@ -83,6 +83,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
// increment time
deltaTij_ += dt;
// Handle first time differently // Handle first time differently
if (k_ == 0) { if (k_ == 0) {
initPosterior(correctedAcc, correctedOmega, dt); initPosterior(correctedAcc, correctedOmega, dt);
@ -140,6 +143,17 @@ NavState PreintegratedMeasurements2::predict(
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
// TODO(frank): handle bias // TODO(frank): handle bias
Vector9 zeta = currentEstimate(); Vector9 zeta = currentEstimate();
cout << "zeta: " << zeta << endl;
Rot3 Ri = state_i.attitude();
Matrix3 Rit = Ri.transpose();
Vector3 gt = deltaTij_ * p_->n_gravity;
zeta.segment<3>(3) +=
Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
zeta.segment<3>(6) += Rit * gt;
cout << "zeta: " << zeta << endl;
cout << "tij: " << deltaTij_ << endl;
cout << "gt: " << gt.transpose() << endl;
cout << "gt^2/2: " << 0.5 * deltaTij_ * gt.transpose() << endl;
return state_i.expmap(zeta); return state_i.expmap(zeta);
} }

20
gtsam/navigation/ScenarioRunner.h
View File

@ -40,27 +40,29 @@ class GaussianBayesNet;
* See ImuFactor.lyx for the relevant math. * See ImuFactor.lyx for the relevant math.
*/ */
class PreintegratedMeasurements2 { class PreintegratedMeasurements2 {
public:
typedef ImuFactor::PreintegratedMeasurements::Params Params;
private: private:
SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; const boost::shared_ptr<Params> p_;
const 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
double deltaTij_; ///< sum of time increments
/// posterior on current iterate, as a conditional P(zeta|bias_delta): /// posterior on current iterate, as a conditional P(zeta|bias_delta):
boost::shared_ptr<GaussianBayesNet> posterior_k_; boost::shared_ptr<GaussianBayesNet> posterior_k_;
public: public:
typedef ImuFactor::PreintegratedMeasurements::Params Params;
Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
PreintegratedMeasurements2( PreintegratedMeasurements2(
const boost::shared_ptr<Params>& p, const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias()) const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias())
: accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), : p_(p),
accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
estimatedBias_(estimatedBias), estimatedBias_(estimatedBias),
k_(0) {} k_(0),
deltaTij_(0.0) {}
/** /**
* Add a single IMU measurement to the preintegration. * Add a single IMU measurement to the preintegration.
@ -76,6 +78,8 @@ class PreintegratedMeasurements2 {
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;
Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
private: private:
// initialize posterior with first (corrected) IMU measurement // initialize posterior with first (corrected) IMU measurement
void initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, void initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega,

195
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -56,14 +56,12 @@ TEST(ScenarioRunner, Spin) {
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
} }
/* ************************************************************************* /* ************************************************************************* */
/*/
namespace forward { namespace forward {
const double v = 2; // m/s const double v = 2; // m/s
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
} }
/* ************************************************************************* /* ************************************************************************* */
/*/
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {
using namespace forward; using namespace forward;
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
@ -76,119 +74,114 @@ TEST(ScenarioRunner, Forward) {
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
} }
///* ************************************************************************* /* ************************************************************************* */
///*/ TEST(ScenarioRunner, ForwardWithBias) {
// TEST(ScenarioRunner, ForwardWithBias) {
// using namespace forward; // using namespace forward;
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); // ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// const double T = 0.1; // seconds // const double T = 0.1; // seconds
// //
// auto pim = runner.integrate(T, kNonZeroBias); // auto pim = runner.integrate(T, kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000,
// kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//} }
//
///* ************************************************************************* /* ************************************************************************* */
///*/ TEST(ScenarioRunner, Circle) {
// TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec
// // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree;
// const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
// ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds
// const double T = 0.1; // seconds
// auto pim = runner.integrate(T);
// auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
//} }
//
///* ************************************************************************* /* ************************************************************************* */
///*/ TEST(ScenarioRunner, Loop) {
// TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s
// // Forward velocity 2m/s // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
// // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) const double v = 2, w = 6 * kDegree;
// const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
// ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds
// const double T = 0.1; // seconds
// auto pim = runner.integrate(T);
// auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
//} }
//
///* ************************************************************************* /* *************************************************************************
///*/ /*/
// namespace initial { namespace initial {
//// Set up body pointing towards y axis, and start at 10,20,0 with velocity // Set up body pointing towards y axis, and start at 10,20,0 with velocity
//// going in X. The body itself has Z axis pointing down // going in X. The body itself has Z axis pointing down
// const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
// const Point3 P0(10, 20, 0); const Point3 P0(10, 20, 0);
// const Vector3 V0(50, 0, 0); const Vector3 V0(50, 0, 0);
//} }
//
///* ************************************************************************* /* *************************************************************************
///*/ /*/
// namespace accelerating { namespace accelerating {
// using namespace initial; using namespace initial;
// const double a = 0.2; // m/s^2 const double a = 0.2; // m/s^2
// const Vector3 A(0, a, 0); const Vector3 A(0, a, 0);
// const AcceleratingScenario scenario(nRb, P0, V0, A); const AcceleratingScenario scenario(nRb, P0, V0, A);
//
// const double T = 3; // seconds const double T = 3; // seconds
//} }
//
///* ************************************************************************* /* ************************************************************************* */
///*/ TEST(ScenarioRunner, Accelerating) {
// TEST(ScenarioRunner, Accelerating) { using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
// TODO(frank):Fails !
// TEST(ScenarioRunner, AcceleratingWithBias) {
// using namespace accelerating; // using namespace accelerating;
// ScenarioRunner runner(&scenario, defaultParams(), T / 10); // ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias);
// //
// auto pim = runner.integrate(T); // auto pim = runner.integrate(T,
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); // kNonZeroBias);
// // Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//} //}
//
///* ************************************************************************* /* ************************************************************************* */
///*/ TEST(ScenarioRunner, AcceleratingAndRotating) {
//// TODO(frank):Fails ! using namespace initial;
//// TEST(ScenarioRunner, AcceleratingWithBias) { const double a = 0.2; // m/s^2
//// using namespace accelerating; const Vector3 A(0, a, 0), W(0, 0.1, 0);
//// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, const AcceleratingScenario scenario(nRb, P0, V0, A, W);
//// kNonZeroBias);
//// const double T = 3; // seconds
//// auto pim = runner.integrate(T, ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//// kNonZeroBias);
//// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, auto pim = runner.integrate(T);
//// kNonZeroBias); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
//// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
////}
//
///* *************************************************************************
///*/
// TEST(ScenarioRunner, AcceleratingAndRotating) {
// using namespace initial;
// const double a = 0.2; // m/s^2
// const Vector3 A(0, a, 0), W(0, 0.1, 0);
// const AcceleratingScenario scenario(nRb, P0, V0, A, W);
//
// const double T = 3; // seconds
// ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//
// auto pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {