def plotGyroFits(gyroDataSet,
misalignmentsAndScales,
skew,
shift,
targetKnots=None):
figureNumber = 0
statisticsByLabelBeforeMotion, samplesByLabelInMotion = gyroDataSet
correctedMeasures = utils.extract_and_correct_gyro_motion(
misalignmentsAndScales, statisticsByLabelBeforeMotion,
samplesByLabelInMotion)
timestamps = sp.array(gyroDataSet[1]['timestamps']) * skew
timestamps += sp.ones_like(timestamps) * shift
if targetKnots is not None:
plots.plot_targets(
regression_gyro.computeTargetVelocities(targetKnots, timestamps),
timestamps, figureNumber, (timestamps[0], timestamps[-1]), math.pi)
plots.plot_motion(correctedMeasures, timestamps,
"Gyro measurements vs CNC targets", figureNumber,
(timestamps[0], timestamps[-1]), math.pi)
figureNumber += 1
plots.show_figures()
def fit_filter_parameters():
statisticsByLabelBeforeMotion, samplesByLabelInMotion = io.deserialize_results(
)
misalignmentsAndScales, skew, shift = io.deserialize_results(
io._pkl_path, "gyro_params_latest.pkl")
gyroMeasures = utils.extract_and_correct_gyro_motion(
misalignmentsAndScales, statisticsByLabelBeforeMotion,
samplesByLabelInMotion)
misalignmentsAndScales, biases = io.deserialize_results(
io._pkl_path, "accelero_params_latest.pkl")
accMeasures = utils.extract_and_correct_accelero_motion(
misalignmentsAndScales, biases, samplesByLabelInMotion)
measures = sp.r_[accMeasures, gyroMeasures]
utils.rescale_gyro_motion_timestamps(samplesByLabelInMotion, _timescale)
imuTimestamps = samplesByLabelInMotion['timestamps']
timestamps = sp.array(imuTimestamps) * skew
timestamps += sp.ones_like(timestamps) * shift
(xSplineTCK, ySplineTCK, zSplineTCK, aSplineTCK,
bSplineTCK) = utils.extract_targets_from_hal_logs_xyzabvt()
x = sp.r_[interpolate.splev(timestamps, xSplineTCK, der=0)]
y = sp.r_[interpolate.splev(timestamps, ySplineTCK, der=0)]
z = sp.r_[interpolate.splev(timestamps, zSplineTCK, der=0)]
a = sp.r_[interpolate.splev(timestamps, aSplineTCK, der=0)]
b = sp.r_[interpolate.splev(timestamps, bSplineTCK, der=0)]
trg = sp.r_['0,2', x, y, z, a, b]
plots.plot_encoders(trg, timestamps)
plots.show_figures()
filterParams = slave_model_filter.fit(sp.r_['0,2', x, y, z, a, b],
measures, timestamps)
#
io.serialize_results(filterParams, io._pkl_path, "filter_params_%s.pkl")
def _testAcquisitionAndPlot(self):
self.acquisition.flushBuffer()
seconds = 50.0
samplesByLabel = self.acquisition.acquireSamplesDuring(seconds)
# io.serialize_results(samplesByLabel)
utils.addTimestamps(samplesByLabel, 64.0 / 16.0e6, True)
self.assertAlmostEqual(utils.computeTimespan(samplesByLabel), seconds,
3)
print utils.computeTimespan(samplesByLabel)
plots.plot_samplesByLabel(samplesByLabel, 'timestamps')
plots.show_figures()
def gyro_dataset(misalignmentsAndScales=None, biases=None):
acquisition = buildAcquisition()
acquisition.flushBuffer()
samplesByLabelBeforeMotion = acquisition.acquireSamplesDuring(5.0)
acquisition.flushBuffer()
samplesByLabelInMotion = acquisition.acquireSamplesDuring(5.0)
dataSet = (samplesByLabelBeforeMotion, samplesByLabelInMotion)
# io.serialize_results(dataSet)
plots.plot_samplesByLabel(
samplesByLabelInMotion, 's',
['b', 'c', 's', 't', 'timestamps', 'T', 'x', 'y', 'z'])
plots.show_figures()
return dataSet
def fit_accelero_parameters_regression():
dataSet = io.float_columns_from_CSV(columnsMask=('longitude', 'latitude',
'avgX', 'avgY', 'avgZ'))
measures = utils.build_measures_matrix(dataSet['avgX'], dataSet['avgY'],
dataSet['avgZ'])
longitudes, latitudes = dataSet["longitude"], dataSet["latitude"]
targets = utils.build_targets_matrix(longitudes, latitudes)
misalignmentsAndScales, biases = regression_accelero.fit(targets, measures)
io.serialize_results((misalignmentsAndScales, biases), io._pkl_path,
"accelero_params_%s.pkl")
correctedMesaures = utils.correct_measures(misalignmentsAndScales, biases,
measures)
residuals = regression_accelero.residuals(
utils.concatenate(misalignmentsAndScales, biases), targets, measures)
# plots.plot_residuals(residuals, longitudes, latitudes)
plots.plot_fit(correctedMesaures, longitudes, latitudes)
plots.show_figures()