From 744727707ebeff9c017a0f2fa8f4240a2884b607 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Jun 2020 12:15:08 -0500 Subject: [PATCH] add guassian noise to initial estimates in Imu example --- cython/gtsam/examples/ImuFactorExample.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 6cc8979f1..ca38e9173 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -76,8 +76,14 @@ class ImuFactorExample(PreintegrationExample): initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) - initial.insert(X(i), state_i.pose()) - initial.insert(V(i), state_i.velocity()) + + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + pose = state_i.pose().compose(poseNoise) + + velocity = state_i.velocity() + np.random.randn(3)*0.1 + + initial.insert(X(i), pose) + initial.insert(V(i), velocity) # simulate the loop i = 0 # state index @@ -88,6 +94,12 @@ class ImuFactorExample(PreintegrationExample): measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + + actual_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + # Plot IMU many times if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) @@ -133,6 +145,9 @@ class ImuFactorExample(PreintegrationExample): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 + + gtsam.utils.plot.set_axes_equal(POSES_FIG) + print(result.atimuBias_ConstantBias(BIAS_KEY)) plt.ioff()