Correct for gravity and V0
Frank Dellaert
parent
06b1f381ea
commit
d3d3b8399d
|
|
@ -83,6 +83,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
|
|||
Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
|
||||
Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
|
||||
|
||||
// increment time
|
||||
deltaTij_ += dt;
|
||||
|
||||
// Handle first time differently
|
||||
if (k_ == 0) {
|
||||
initPosterior(correctedAcc, correctedOmega, dt);
|
||||
|
|
@ -140,6 +143,17 @@ NavState PreintegratedMeasurements2::predict(
|
|||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
|
||||
// TODO(frank): handle bias
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -40,27 +40,29 @@ class GaussianBayesNet;
|
|||
* See ImuFactor.lyx for the relevant math.
|
||||
*/
|
||||
class PreintegratedMeasurements2 {
|
||||
public:
|
||||
typedef ImuFactor::PreintegratedMeasurements::Params Params;
|
||||
|
||||
private:
|
||||
SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
|
||||
const boost::shared_ptr<Params> p_;
|
||||
const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
|
||||
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):
|
||||
boost::shared_ptr<GaussianBayesNet> posterior_k_;
|
||||
|
||||
public:
|
||||
typedef ImuFactor::PreintegratedMeasurements::Params Params;
|
||||
|
||||
Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
|
||||
|
||||
PreintegratedMeasurements2(
|
||||
const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias())
|
||||
: accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
|
||||
: p_(p),
|
||||
accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
|
||||
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
|
||||
estimatedBias_(estimatedBias),
|
||||
k_(0) {}
|
||||
k_(0),
|
||||
deltaTij_(0.0) {}
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
|
|
@ -76,6 +78,8 @@ class PreintegratedMeasurements2 {
|
|||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||
|
||||
Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
|
||||
|
||||
private:
|
||||
// initialize posterior with first (corrected) IMU measurement
|
||||
void initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega,
|
||||
|
|
|
|||
|
|
@ -56,14 +56,12 @@ TEST(ScenarioRunner, Spin) {
|
|||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
/*/
|
||||
/* ************************************************************************* */
|
||||
namespace forward {
|
||||
const double v = 2; // m/s
|
||||
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
|
||||
}
|
||||
/* *************************************************************************
|
||||
/*/
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Forward) {
|
||||
using namespace forward;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
|
|
@ -76,120 +74,115 @@ TEST(ScenarioRunner, Forward) {
|
|||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
}
|
||||
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// TEST(ScenarioRunner, ForwardWithBias) {
|
||||
// using namespace forward;
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
// const double T = 0.1; // seconds
|
||||
//
|
||||
// auto pim = runner.integrate(T, kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// TEST(ScenarioRunner, Circle) {
|
||||
// // Forward velocity 2m/s, angular velocity 6 kDegree/sec
|
||||
// const double v = 2, w = 6 * kDegree;
|
||||
// ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||
//
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
// const double T = 0.1; // seconds
|
||||
//
|
||||
// auto pim = runner.integrate(T);
|
||||
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
//
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// TEST(ScenarioRunner, Loop) {
|
||||
// // Forward velocity 2m/s
|
||||
// // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||
// const double v = 2, w = 6 * kDegree;
|
||||
// ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
//
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
// const double T = 0.1; // seconds
|
||||
//
|
||||
// auto pim = runner.integrate(T);
|
||||
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
//
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// namespace initial {
|
||||
//// 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
|
||||
// const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
// const Point3 P0(10, 20, 0);
|
||||
// const Vector3 V0(50, 0, 0);
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// namespace accelerating {
|
||||
// using namespace initial;
|
||||
// const double a = 0.2; // m/s^2
|
||||
// const Vector3 A(0, a, 0);
|
||||
// const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||
//
|
||||
// const double T = 3; // seconds
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// TEST(ScenarioRunner, Accelerating) {
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, ForwardWithBias) {
|
||||
// using namespace forward;
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
// const double T = 0.1; // seconds
|
||||
//
|
||||
// auto pim = runner.integrate(T, kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000,
|
||||
// kNonZeroBias);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Circle) {
|
||||
// Forward velocity 2m/s, angular velocity 6 kDegree/sec
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Loop) {
|
||||
// Forward velocity 2m/s
|
||||
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
/*/
|
||||
namespace initial {
|
||||
// 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
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 P0(10, 20, 0);
|
||||
const Vector3 V0(50, 0, 0);
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
/*/
|
||||
namespace accelerating {
|
||||
using namespace initial;
|
||||
const double a = 0.2; // m/s^2
|
||||
const Vector3 A(0, a, 0);
|
||||
const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||
|
||||
const double T = 3; // seconds
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
|
||||
// kNonZeroBias);
|
||||
//
|
||||
// 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;
|
||||
//// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
|
||||
//// kNonZeroBias);
|
||||
////
|
||||
//// auto pim = runner.integrate(T,
|
||||
//// kNonZeroBias);
|
||||
//// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
//// kNonZeroBias);
|
||||
//// 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);
|
||||
// auto pim = runner.integrate(T,
|
||||
// kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
// kNonZeroBias);
|
||||
// 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);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue