33 lines
898 B
Python
33 lines
898 B
Python
"""
|
|
A script validating the ImuFactor inference.
|
|
"""
|
|
|
|
import math
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
from mpl_toolkits.mplot3d import Axes3D
|
|
|
|
import gtsam
|
|
from gtsam_utils import plotPose3
|
|
from PreintegrationExample import PreintegrationExample
|
|
|
|
class ImuFactorExample(PreintegrationExample):
|
|
|
|
def run(self):
|
|
# simulate the loop up to the top
|
|
T = self.timeForOneLoop
|
|
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
|
|
for i, t in enumerate(np.arange(0, T, self.dt)):
|
|
measuredOmega = self.runner.measuredAngularVelocity(t)
|
|
measuredAcc = self.runner.measuredSpecificForce(t)
|
|
if i % 25 == 0:
|
|
self.plotImu(t, measuredOmega, measuredAcc)
|
|
self.plotGroundTruthPose(t)
|
|
|
|
plt.ioff()
|
|
plt.show()
|
|
|
|
if __name__ == '__main__':
|
|
ImuFactorExample().run()
|