2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 A script validating the Preintegration of IMU measurements.
10 Authors: Frank Dellaert, Varun Agrawal.
15 from typing
import Optional, Sequence
18 import matplotlib.pyplot
as plt
21 from mpl_toolkits.mplot3d
import Axes3D
29 """Base class for all preintegration examples."""
32 """Create default parameters with Z *up* and realistic noise parameters"""
34 kGyroSigma = np.radians(0.5) / 60
35 kAccelSigma = 0.1 / 60
36 params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float))
37 params.setAccelerometerCovariance(kAccelSigma**2 *
38 np.identity(3, float))
39 params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
43 twist: Optional[np.ndarray] =
None,
47 """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
58 W = np.array([0, -np.radians(30), 0])
59 V = np.array([2, 0, 0])
77 accBias = np.array([0, 0.1, 0])
78 gyroBias = np.array([0, 0, 0])
85 fig, self.
axes = plt.subplots(4, 3)
86 fig.set_tight_layout(
True)
88 def plotImu(self, t: float, measuredOmega: Sequence,
89 measuredAcc: Sequence):
91 Plot IMU measurements.
93 t: The time at which the measurement was recoreded.
94 measuredOmega: Measured angular velocity.
95 measuredAcc: Measured linear acceleration.
101 for i, (label, color)
in enumerate(zip(self.
labels, self.
colors)):
103 ax.scatter(t, omega_b[i], color=
'k', marker=
'.')
104 ax.scatter(t, measuredOmega[i], color=color, marker=
'.')
105 ax.set_xlabel(
'angular velocity ' + label)
108 acceleration_n = self.
scenario.acceleration_n(t)
109 for i, (label, color)
in enumerate(zip(self.
labels, self.
colors)):
111 ax.scatter(t, acceleration_n[i], color=color, marker=
'.')
112 ax.set_xlabel(
'acceleration in nav ' + label)
115 acceleration_b = self.
scenario.acceleration_b(t)
116 for i, (label, color)
in enumerate(zip(self.
labels, self.
colors)):
118 ax.scatter(t, acceleration_b[i], color=color, marker=
'.')
119 ax.set_xlabel(
'acceleration in body ' + label)
122 actual = self.
runner.actualSpecificForce(t)
123 for i, (label, color)
in enumerate(zip(self.
labels, self.
colors)):
125 ax.scatter(t, actual[i], color=
'k', marker=
'.')
126 ax.scatter(t, measuredAcc[i], color=color, marker=
'.')
127 ax.set_xlabel(
'specific force ' + label)
132 time_interval: float = 0.01):
134 Plot ground truth pose, as well as prediction from integrated IMU measurements.
136 t: Time at which the pose was obtained.
137 scale: The scaling factor for the pose axes.
138 time_interval: The time to wait before showing the plot.
142 translation = actualPose.translation()
149 plt.pause(time_interval)
151 def run(self, T: int = 12):
152 """Simulate the loop."""
153 for i, t
in enumerate(np.arange(0, T, self.
dt)):
154 measuredOmega = self.
runner.measuredAngularVelocity(t)
155 measuredAcc = self.
runner.measuredSpecificForce(t)
157 self.
plotImu(t, measuredOmega, measuredAcc)
161 plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
167 if __name__ ==
'__main__':