#odometry = np.column_stack((odometry_velocity.getAxis(0), odometry_velocity.getAxis(1), odometry_velocity.getAxis(2))) gp_odometry = np.column_stack((gp_odometry_position.getAxis(0), gp_odometry_position.getAxis(1), gp_odometry_position.getAxis(2))) #gp_odometry = np.column_stack((gp_odometry_velocity.getAxis(0), gp_odometry_velocity.getAxis(1), gp_odometry_velocity.getAxis(2))) ################## ## Block Filter ## ################## sampling_frequency = 1.0/mean(reference_position.delta[0:100]) size_block = 4 * sampling_frequency number_blocks = int(len(reference_position.delta)/size_block) # Split inertia (one axis info per column) inertia, inertiastd = data.input_reduction(inertia, number_blocks) # Split orientation (one axis info per column) orient, orientstd = data.input_reduction(orient, number_blocks) # Split reference (one axis info per column) reference, referencestd = data.input_reduction(reference, number_blocks) # Split odometry (one axis info per column) odometry, odometrystd = data.input_reduction(odometry, number_blocks) # Split odometry (one axis info per column) gp_odometry, gp_odometrystd = data.input_reduction(gp_odometry, number_blocks)
#reference = np.column_stack((reference_position.getAxis(0), reference_position.getAxis(1), reference_position.getAxis(2))) reference = np.column_stack((reference_velocity.getAxis(0), reference_velocity.getAxis(1), reference_velocity.getAxis(2))) #odometry = np.column_stack((odometry_position.getAxis(0), odometry_position.getAxis(1), odometry_position.getAxis(2))) odometry = np.column_stack((odometry_velocity.getAxis(0), odometry_velocity.getAxis(1), odometry_velocity.getAxis(2))) ######################### ## SPLIT INPUT TEST ## ######################### sampling_frequency = 1.0/mean(reference_velocity.delta[0:100]) size_block = 5 * sampling_frequency number_blocks = int(len(reference_velocity.delta)/size_block) # Split joints (one joint info per column) joints, jointstd = data.input_reduction(joints, number_blocks) # Split inertia (one axis info per column) inertia, inertiastd = data.input_reduction(inertia, number_blocks) # Split orientation (one axis info per column) orient, orientstd = data.input_reduction(orient, number_blocks) # Split reference (one axis info per column) reference, referencestd = data.input_reduction(reference, number_blocks) # Split odometry (one axis info per column) odometry, odometrystd = data.input_reduction(odometry, number_blocks) error = np.absolute(reference - odometry)
## LOAD OUTPUT ##
###################
variance = np.column_stack(
(gp_odometry_velocity.var[:, 0], gp_odometry_velocity.var[:, 1], gp_odometry_velocity.var[:, 2])
)
std_deviation = np.column_stack((np.sqrt(variance[:, 0]), np.sqrt(variance[:, 1]), np.sqrt(variance[:, 2])))
#########################
## SPLIT INPUT TEST ##
#########################
sampling_frequency = 1.0 / mean(gp_odometry_velocity.delta[0:100])
size_block = 4 * sampling_frequency
number_blocks = int(len(gp_odometry_velocity.delta) / size_block)
# Split joints (one joint info per column)
joints, jointstd = data.input_reduction(joints, number_blocks)
# Split inertia (one axis info per column)
inertia, inertiastd = data.input_reduction(inertia, number_blocks)
# Split orientation (one axis info per column)
orient, orientstd = data.input_reduction(orient, number_blocks)
# Split std deviation (one axis info per column)
deviation, deviationstd = data.input_reduction(std_deviation, number_blocks)
#########################
## GAUSSIAN PROCESS ##
#########################
# GP Multidimensional Input
