From b6ead53c2530f145ef45f54553c4c752729c1f16 Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Wed, 27 Jan 2016 11:50:36 -0800
Subject: [PATCH] Validate bias correction

---
 python/gtsam_examples/ImuFactorExample.py  | 11 +++++++----
 python/handwritten/navigation/Scenario.cpp |  6 ++++--
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py
index 05399df08..c2432673e 100644
--- a/python/gtsam_examples/ImuFactorExample.py
+++ b/python/gtsam_examples/ImuFactorExample.py
@@ -48,8 +48,11 @@ class ImuFactorExample(object):
         self.g = 10  # simple gravity constant
         self.params = self.defaultParams(self.g)
         ptr = gtsam.ScenarioPointer(self.scenario)
-        self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt)
-        self.estimatedBias = gtsam.ConstantBias()
+        accBias = np.array([0, 0.1, 0])
+        gyroBias = np.array([0, 0, 0])
+        self.actualBias = gtsam.ConstantBias(accBias, gyroBias)
+        print(self.actualBias)
+        self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias)
 
     def plot(self, t, measuredOmega, measuredAcc):
         # plot angular velocity    
@@ -80,8 +83,8 @@ class ImuFactorExample(object):
         # plot ground truth pose, as well as prediction from integrated IMU measurements
         actualPose = self.scenario.pose(t)
         plotPose3(4, actualPose, 0.3)
-        pim = self.runner.integrate(t, self.estimatedBias, False)
-        predictedNavState = self.runner.predict(pim, self.estimatedBias)
+        pim = self.runner.integrate(t, self.actualBias, True)
+        predictedNavState = self.runner.predict(pim, self.actualBias)
         plotPose3(4, predictedNavState.pose(), 0.1)
         ax = plt.gca()
         ax.set_xlim3d(-self.radius, self.radius)
diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp
index 05d2edf4d..936d4ef18 100644
--- a/python/handwritten/navigation/Scenario.cpp
+++ b/python/handwritten/navigation/Scenario.cpp
@@ -80,12 +80,14 @@ void exportScenario() {
       .def(repr(self))
       .def("pose", &NavState::pose);
 
-  class_<imuBias::ConstantBias>("ConstantBias", init<>());
+  class_<imuBias::ConstantBias>("ConstantBias", init<>())
+      .def(init<const Vector3&, const Vector3&>())
+      .def(repr(self));
 
   class_<ScenarioRunner>(
       "ScenarioRunner",
       init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
-           double>())
+           double, const imuBias::ConstantBias&>())
       .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
       .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
       .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce)