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

Validate bias correction

release/4.3a0
Frank 2016-01-27 11:50:36 -08:00
parent e8565d27f7
commit b6ead53c25
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
python/gtsam_examples/ImuFactorExample.py
View File

@ -48,8 +48,11 @@ class ImuFactorExample(object):
self.g = 10 # simple gravity constant self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g) self.params = self.defaultParams(self.g)
ptr = gtsam.ScenarioPointer(self.scenario) ptr = gtsam.ScenarioPointer(self.scenario)
self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt) accBias = np.array([0, 0.1, 0])
self.estimatedBias = gtsam.ConstantBias() 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): def plot(self, t, measuredOmega, measuredAcc):
# plot angular velocity # plot angular velocity
@ -80,8 +83,8 @@ class ImuFactorExample(object):
# plot ground truth pose, as well as prediction from integrated IMU measurements # plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = self.scenario.pose(t) actualPose = self.scenario.pose(t)
plotPose3(4, actualPose, 0.3) plotPose3(4, actualPose, 0.3)
pim = self.runner.integrate(t, self.estimatedBias, False) pim = self.runner.integrate(t, self.actualBias, True)
predictedNavState = self.runner.predict(pim, self.estimatedBias) predictedNavState = self.runner.predict(pim, self.actualBias)
plotPose3(4, predictedNavState.pose(), 0.1) plotPose3(4, predictedNavState.pose(), 0.1)
ax = plt.gca() ax = plt.gca()
ax.set_xlim3d(-self.radius, self.radius) ax.set_xlim3d(-self.radius, self.radius)

6
python/handwritten/navigation/Scenario.cpp
View File

@ -80,12 +80,14 @@ void exportScenario() {
.def(repr(self)) .def(repr(self))
.def("pose", &NavState::pose); .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>( class_<ScenarioRunner>(
"ScenarioRunner", "ScenarioRunner",
init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&, init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
double>()) double, const imuBias::ConstantBias&>())
.def("actualSpecificForce", &ScenarioRunner::actualSpecificForce) .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
.def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity) .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
.def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce) .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce)