PreintegrationExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 A script validating the Preintegration of IMU measurements
9 """
10 
11 import math
12 
13 import gtsam
14 import matplotlib.pyplot as plt
15 import numpy as np
16 from gtsam.utils.plot import plot_pose3
17 from mpl_toolkits.mplot3d import Axes3D
18 
19 IMU_FIG = 1
20 POSES_FIG = 2
21 
22 
24 
25  @staticmethod
26  def defaultParams(g):
27  """Create default parameters with Z *up* and realistic noise parameters"""
29  kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
30  kAccelSigma = 0.1 / 60 # 10 cm VRW
31  params.setGyroscopeCovariance(
32  kGyroSigma ** 2 * np.identity(3, float))
33  params.setAccelerometerCovariance(
34  kAccelSigma ** 2 * np.identity(3, float))
35  params.setIntegrationCovariance(
36  0.0000001 ** 2 * np.identity(3, float))
37  return params
38 
39  def __init__(self, twist=None, bias=None, dt=1e-2):
40  """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
41 
42  # setup interactive plotting
43  plt.ion()
44 
45  # Setup loop as default scenario
46  if twist is not None:
47  (W, V) = twist
48  else:
49  # default = loop with forward velocity 2m/s, while pitching up
50  # with angular velocity 30 degree/sec (negative in FLU)
51  W = np.array([0, -math.radians(30), 0])
52  V = np.array([2, 0, 0])
53 
55  self.dt = dt
56 
57  self.maxDim = 5
58  self.labels = list('xyz')
59  self.colors = list('rgb')
60 
61  # Create runner
62  self.g = 10 # simple gravity constant
63  self.params = self.defaultParams(self.g)
64 
65  if bias is not None:
66  self.actualBias = bias
67  else:
68  accBias = np.array([0, 0.1, 0])
69  gyroBias = np.array([0, 0, 0])
70  self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
71 
73  self.scenario, self.params, self.dt, self.actualBias)
74 
75  fig, self.axes = plt.subplots(4, 3)
76  fig.set_tight_layout(True)
77 
78  def plotImu(self, t, measuredOmega, measuredAcc):
79  plt.figure(IMU_FIG)
80 
81  # plot angular velocity
82  omega_b = self.scenario.omega_b(t)
83  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
84  ax = self.axes[0][i]
85  ax.scatter(t, omega_b[i], color='k', marker='.')
86  ax.scatter(t, measuredOmega[i], color=color, marker='.')
87  ax.set_xlabel('angular velocity ' + label)
88 
89  # plot acceleration in nav
90  acceleration_n = self.scenario.acceleration_n(t)
91  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
92  ax = self.axes[1][i]
93  ax.scatter(t, acceleration_n[i], color=color, marker='.')
94  ax.set_xlabel('acceleration in nav ' + label)
95 
96  # plot acceleration in body
97  acceleration_b = self.scenario.acceleration_b(t)
98  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
99  ax = self.axes[2][i]
100  ax.scatter(t, acceleration_b[i], color=color, marker='.')
101  ax.set_xlabel('acceleration in body ' + label)
102 
103  # plot actual specific force, as well as corrupted
104  actual = self.runner.actualSpecificForce(t)
105  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
106  ax = self.axes[3][i]
107  ax.scatter(t, actual[i], color='k', marker='.')
108  ax.scatter(t, measuredAcc[i], color=color, marker='.')
109  ax.set_xlabel('specific force ' + label)
110 
111  def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
112  # plot ground truth pose, as well as prediction from integrated IMU measurements
113  actualPose = self.scenario.pose(t)
114  plot_pose3(POSES_FIG, actualPose, scale)
115  t = actualPose.translation()
116  self.maxDim = max([max(np.abs(t)), self.maxDim])
117  ax = plt.gca()
118  ax.set_xlim3d(-self.maxDim, self.maxDim)
119  ax.set_ylim3d(-self.maxDim, self.maxDim)
120  ax.set_zlim3d(-self.maxDim, self.maxDim)
121 
122  plt.pause(time_interval)
123 
124  def run(self, T=12):
125  # simulate the loop
126  for i, t in enumerate(np.arange(0, T, self.dt)):
127  measuredOmega = self.runner.measuredAngularVelocity(t)
128  measuredAcc = self.runner.measuredSpecificForce(t)
129  if i % 25 == 0:
130  self.plotImu(t, measuredOmega, measuredAcc)
131  self.plotGroundTruthPose(t)
132  pim = self.runner.integrate(t, self.actualBias, True)
133  predictedNavState = self.runner.predict(pim, self.actualBias)
134  plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
135 
136  plt.ioff()
137  plt.show()
138 
139 
140 if __name__ == '__main__':
#define max(a, b)
Definition: datatypes.h:20
static boost::shared_ptr< PreintegrationParams > MakeSharedU(double g=9.81)
def plot_pose3(fignum, pose, axis_length=0.1, P=None, axis_labels=('X axis', 'Y axis', 'Z axis'))
Definition: plot.py:284
Definition: pytypes.h:1301
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01)


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:29