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

AcceleratingScenario + some refactoring (v and a specified in nav frame)

release/4.3a0
Frank Dellaert 2015-12-23 11:15:42 -08:00
parent dfe3f3a348
commit 00b83ced7a
4 changed files with 118 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
gtsam/navigation/Scenario.h
View File

@ -54,25 +54,25 @@ class Scenario {
Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
/// Pose of body in nav frame at time t
virtual Pose3 pose(double t) const = 0;
// Quantities a Scenario needs to specify:
virtual Pose3 pose(double t) const = 0;
virtual Vector3 omega_b(double t) const = 0;
virtual Vector3 velocity_n(double t) const = 0;
virtual Vector3 acceleration_n(double t) const = 0;
// Derived quantities:
/// Rotation of body in nav frame at time t
virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
virtual Vector3 angularVelocityInBody(double t) const = 0;
virtual Vector3 linearVelocityInBody(double t) const = 0;
virtual Vector3 accelerationInBody(double t) const = 0;
/// Velocity in nav frame at time t
Vector3 velocity(double t) const {
return rotation(t) * linearVelocityInBody(t);
Vector3 velocity_b(double t) const {
const Rot3 nRb = rotation(t);
return nRb.transpose() * velocity_n(t);
}
// acceleration in nav frame
// TODO(frank): correct for rotating frames?
Vector3 acceleration(double t) const {
return rotation(t) * accelerationInBody(t);
Vector3 acceleration_b(double t) const {
const Rot3 nRb = rotation(t);
return nRb.transpose() * acceleration_n(t);
}
private:
@ -80,9 +80,7 @@ class Scenario {
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
};
/**
* Simple IMU simulator with constant twist 3D trajectory.
*/
/// Scenario with constant twist 3D trajectory.
class ExpmapScenario : public Scenario {
public:
/// Construct scenario with constant twist [w,v]
@ -91,21 +89,40 @@ class ExpmapScenario : public Scenario {
double accSigma = 0.01)
: Scenario(imuSampleTime, gyroSigma, accSigma),
twist_((Vector6() << w, v).finished()),
centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {}
a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); }
Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); }
Vector3 accelerationInBody(double t) const {
const Rot3 nRb = rotation(t);
return centripetalAcceleration_ - nRb.transpose() * gravity();
}
Vector3 omega_b(double t) const { return twist_.head<3>(); }
Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); }
Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
private:
const Vector6 twist_;
const Vector3 centripetalAcceleration_;
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
};
/// Accelerating from an arbitrary initial state
class AcceleratingScenario : public Scenario {
public:
/// Construct scenario with constant twist [w,v]
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& accelerationInBody,
double imuSampleTime = 1.0 / 100.0,
double gyroSigma = 0.17, double accSigma = 0.01)
: Scenario(imuSampleTime, gyroSigma, accSigma),
nRb_(nRb),
p0_(p0.vector()),
v0_(v0),
a_n_(nRb_ * accelerationInBody) {}
Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); }
Vector3 omega_b(double t) const { return Vector3::Zero(); }
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
Vector3 acceleration_n(double t) const { return a_n_; }
private:
const Rot3 nRb_;
const Vector3 p0_, v0_, a_n_;
};
} // namespace gtsam

6
gtsam/navigation/ScenarioRunner.h
View File

@ -48,9 +48,9 @@ class ScenarioRunner {
const size_t nrSteps = T / dt;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += dt) {
Vector3 measuredOmega = scenario_->angularVelocityInBody(t);
Vector3 measuredOmega = scenario_->omega_b(t);
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
Vector3 measuredAcc = scenario_->accelerationInBody(t);
Vector3 measuredAcc = scenario_->acceleration_b(t);
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
@ -64,7 +64,7 @@ class ScenarioRunner {
// TODO(frank): allow non-zero bias, omegaCoriolis
const imuBias::ConstantBias zeroBias;
const Pose3 pose_i = Pose3::identity();
const Vector3 vel_i = scenario_->velocity(0);
const Vector3 vel_i = scenario_->velocity_n(0);
const Vector3 omegaCoriolis = Vector3::Zero();
const bool use2ndOrderCoriolis = true;
return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(),

53
gtsam/navigation/tests/testScenario.cpp
View File

@ -27,9 +27,15 @@ static const double degree = M_PI / 180.0;
/* ************************************************************************* */
TEST(Scenario, Forward) {
const double v = 2; // m/s
ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0));
const Vector3 W(0, 0, 0), V(v, 0, 0);
const ExpmapScenario scenario(W, V);
const Pose3 T15 = forward.pose(15);
const double T = 15;
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
const Pose3 T15 = scenario.pose(T);
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
}
@ -38,11 +44,17 @@ TEST(Scenario, Forward) {
TEST(Scenario, Circle) {
// Forward velocity 2m/s, angular velocity 6 degree/sec around Z
const double v = 2, w = 6 * degree;
ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
const Vector3 W(0, 0, w), V(v, 0, 0);
const ExpmapScenario scenario(W, V);
const double T = 15;
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
// R = v/w, so test if circle is of right size
const double R = v / w;
const Pose3 T15 = circle.pose(15);
const Pose3 T15 = scenario.pose(T);
EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
}
@ -52,15 +64,44 @@ TEST(Scenario, Loop) {
// Forward velocity 2m/s
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
const double v = 2, w = 6 * degree;
ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
const Vector3 W(0, -w, 0), V(v, 0, 0);
const ExpmapScenario scenario(W, V);
const double T = 30;
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
// R = v/w, so test if loop crests at 2*R
const double R = v / w;
const Pose3 T30 = loop.pose(30);
const Pose3 T30 = scenario.pose(30);
EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9));
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
}
/* ************************************************************************* */
TEST(Scenario, Accelerating) {
// 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);
const double a_b = 0.2; // m/s^2
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
const double T = 3;
const Vector3 A = nRb * Vector3(a_b, 0, 0);
EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
const Pose3 T3 = scenario.pose(3);
EXPECT(assert_equal(nRb, T3.rotation(), 1e-9));
EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(),
1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -78,6 +78,30 @@ TEST(ScenarioRunner, Loop) {
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
// 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 initial_position(10, 20, 0);
const Vector3 initial_velocity(50, 0, 0);
const double a = 0.2, dt = 3.0;
AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity,
Vector3(a, 0, 0), dt / 10, kGyroSigma,
kAccelerometerSigma);
ScenarioRunner runner(&scenario);
const double T = 3; // seconds
ImuFactor::PreintegratedMeasurements 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), 1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;