diff --git a/python/gtsam/examples/CombinedImuFactorExample.py b/python/gtsam/examples/CombinedImuFactorExample.py index 82714dd7c..e3f8c6b34 100644 --- a/python/gtsam/examples/CombinedImuFactorExample.py +++ b/python/gtsam/examples/CombinedImuFactorExample.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -A script validating and demonstrating the CobiendImuFactor inference. +A script validating and demonstrating inference with the CombinedImuFactor. Author: Varun Agrawal """ @@ -17,15 +17,15 @@ from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D - from PreintegrationExample import POSES_FIG, PreintegrationExample +import gtsam + GRAVITY = 9.81 np.set_printoptions(precision=3, suppress=True) @@ -192,7 +192,9 @@ class CombinedImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(initial_state_i, self.actualBias)) + print("Predicted state at {0}:\n{1}".format( + t + self.dt, + pim.predict(initial_state_i, self.actualBias))) pim.resetIntegration() @@ -204,6 +206,9 @@ class CombinedImuFactorExample(PreintegrationExample): print("Actual state at {0}:\n{1}".format( t + self.dt, actual_state_i)) + # Set initial state to current + initial_state_i = actual_state_i + noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3) * 0.1) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index c86a4e216..a0c833274 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -17,15 +17,15 @@ from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D - from PreintegrationExample import POSES_FIG, PreintegrationExample +import gtsam + BIAS_KEY = B(0) GRAVITY = 9.81 @@ -185,7 +185,9 @@ class ImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(initial_state_i, self.actualBias)) + print("Predicted state at {0}:\n{1}".format( + t + self.dt, + pim.predict(initial_state_i, self.actualBias))) pim.resetIntegration() @@ -197,6 +199,9 @@ class ImuFactorExample(PreintegrationExample): print("Actual state at {0}:\n{1}".format( t + self.dt, actual_state_i)) + # Set initial state to current + initial_state_i = actual_state_i + noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3) * 0.1)