Further examining a circular trajectory

gtsam/navigation/Scenario.h

@@ -20,33 +20,49 @@
 
 namespace gtsam {
 
-/// Simple class with constant twist 3D trajectory
+/**
+ * Simple class with constant twist 3D trajectory.
+ * It is also assumed that gravity is magically counteracted and has no effect
+ * on trajectory. Hence, a simulated IMU yields the actual body angular
+ * velocity, and negative G acceleration plus the acceleration created by the
+ * rotating body frame.
+ */
 class Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
-  Scenario(const Vector3& w, const Vector3& v, double imuSampleTime = 1e-2)
+  Scenario(const Vector3& w, const Vector3& v,
+           double imuSampleTime = 1.0 / 100.0)
       : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {}
 
+  const double& imuSampleTime() const { return imuSampleTime_; }
+
   // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
   // also, uses g=10 for easy debugging
   Vector3 gravity() const { return Vector3(0, 0, -10.0); }
 
-  Vector3 groundTruthGyroInBody() const { return twist_.head<3>(); }
-  Vector3 groundTruthVelocityInBody() const { return twist_.tail<3>(); }
+  Vector3 angularVelocityInBody() const { return twist_.head<3>(); }
+  Vector3 linearVelocityInBody() const { return twist_.tail<3>(); }
 
-  // All constant twist scenarios have zero acceleration
-  Vector3 groundTruthAccInBody() const { return Vector3::Zero(); }
-
-  const double& imuSampleTime() const { return imuSampleTime_; }
+  /// Rotation of body in nav frame at time t
+  Rot3 rotAtTime(double t) const {
+    return Rot3::Expmap(angularVelocityInBody() * t);
+  }
 
   /// Pose of body in nav frame at time t
-  Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); }
+  Pose3 poseAtTime(double t) const { return Pose3::Expmap(twist_ * t); }
 
   /// Velocity in nav frame at time t
   Vector3 velocityAtTime(double t) {
-    const Pose3 pose = poseAtTime(t);
-    const Rot3& nRb = pose.rotation();
-    return nRb * groundTruthVelocityInBody();
+    const Rot3 nRb = rotAtTime(t);
+    return nRb * linearVelocityInBody();
+  }
+
+  // acceleration in nav frame
+  Vector3 accelerationAtTime(double t) const {
+    const Rot3 nRb = rotAtTime(t);
+    const Vector3 centripetalAcceleration =
+        angularVelocityInBody().cross(linearVelocityInBody());
+    return nRb * centripetalAcceleration - gravity();
   }
 
  private:

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -18,6 +18,8 @@
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
 
+#include <iostream>
+
 namespace gtsam {
 
 double accNoiseVar = 0.01;
@@ -42,11 +44,17 @@ class ScenarioRunner {
         zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
         kIntegrationErrorCovariance, use2ndOrderCoriolis);
 
-    const Vector3 measuredAcc = scenario_.groundTruthAccInBody();
-    const Vector3 measuredOmega = scenario_.groundTruthGyroInBody();
-    double deltaT = scenario_.imuSampleTime();
-    for (double t = 0; t <= T; t += deltaT) {
+    const Vector3 measuredOmega = scenario_.angularVelocityInBody();
+    const double deltaT = scenario_.imuSampleTime();
+    const size_t nrSteps = T / deltaT;
+    double t = 0;
+    for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
+      std::cout << t << ", " << deltaT << ": ";
+      const Vector3 measuredAcc = scenario_.accelerationAtTime(t);
       result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+      //      std::cout << result.deltaRij() << std::endl;
+      std::cout << " a:" << measuredAcc.transpose();
+      std::cout << " P:" << result.deltaVij().transpose() << std::endl;
     }
 
     return result;
@@ -87,13 +95,13 @@ using namespace gtsam;
 
 static const double degree = M_PI / 180.0;
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST(ScenarioRunner, Forward) {
   const double v = 2;  // m/s
   Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
 
   ScenarioRunner runner(forward);
-  const double T = 10;  // seconds
+  const double T = 1;  // seconds
   ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
   EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9));
 }
@@ -102,7 +110,7 @@ TEST(ScenarioRunner, Forward) {
 TEST(ScenarioRunner, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec
   const double v = 2, omega = 6 * degree;
-  Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0));
+  Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.1);
 
   ScenarioRunner runner(circle);
   const double T = 15;  // seconds