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

add guassian noise to initial estimates in Imu example

release/4.3a0
Varun Agrawal 2020-06-09 12:15:08 -05:00
parent b11b184f22
commit 744727707e
1 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cython/gtsam/examples/ImuFactorExample.py
View File

@ -76,8 +76,14 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(BIAS_KEY, self.actualBias) initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses): for i in range(num_poses):
state_i = self.scenario.navState(float(i)) 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 # simulate the loop
i = 0 # state index i = 0 # state index
@ -88,6 +94,12 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t) measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) 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 # Plot IMU many times
if k % 10 == 0: if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc) self.plotImu(t, measuredOmega, measuredAcc)
@ -133,6 +145,9 @@ class ImuFactorExample(PreintegrationExample):
pose_i = result.atPose3(X(i)) pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1) plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1 i += 1
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY)) print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff() plt.ioff()