td_okvis.applyBodyTransformToTwist('vel', 'ror',
bodyTransformForBetterPlotRangePos,
bodyTransformForBetterPlotRangeAtt)
td_okvis.applyInertialTransform('pos', 'att', np.zeros(3),
np.array([1.0, 0, 0, 0]))
if True: # Align Vicon to Rovio using calibration (body frame)
B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(
Quaternion.q_inverse(qVM), bodyTransformForBetterPlotRangePos)
qCB_est = Quaternion.q_mult(bodyTransformForBetterPlotRangeAtt,
Quaternion.q_inverse(qVM))
td_vicon.applyBodyTransform('pos', 'att', B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist('vel', 'ror', B_r_BC_est, qCB_est)
if doRovio and bodyAlignViconToRovio: # Align Vicon to Rovio (body frame)
B_r_BC_est, qCB_est = td_vicon.calibrateBodyTransform(
'vel', 'ror', td_rovio, 'vel', 'ror')
print('Calibrate Body Transform for Rovio:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' +
str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' +
str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' +
str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
td_vicon.applyBodyTransform('pos', 'att', B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist('vel', 'ror', B_r_BC_est, qCB_est)
if doOkvis and bodyAlignOkvisToVicon: # Align Okvis to Vicon (body frame)
B_r_BC_est, qCB_est = td_okvis.calibrateBodyTransform(
'vel', 'ror', td_vicon, 'vel', 'ror')
print('Calibrate Body Transform for Okvis:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' +
str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' +
td_rovio.applyBodyTransformToAttCov(rovio_attCovID, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToTwist(rovio_velID, rovio_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyInertialTransform(rovio_posID, rovio_attID, np.zeros(3), np.array([0,0,0,1]))
if doOkvis:
td_okvis.applyBodyTransform(okvis_posID, okvis_attID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyBodyTransformToTwist(okvis_velID, okvis_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyInertialTransform(okvis_posID, okvis_attID, np.zeros(3), np.array([0,0,0,1]))
if True: # Align Vicon to Rovio using calibration (body frame)
B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(Quaternion.q_inverse(qVM),bodyTransformForBetterPlotRangePos)
qCB_est = Quaternion.q_mult(bodyTransformForBetterPlotRangeAtt,Quaternion.q_inverse(qVM))
td_vicon.applyBodyTransform(vicon_posID, vicon_attID, B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist(vicon_velID, vicon_rorID, B_r_BC_est, qCB_est)
if doRovio and bodyAlignViconToRovio: # Align Vicon to Rovio (body frame)
B_r_BC_est, qCB_est = td_vicon.calibrateBodyTransform(vicon_velID, vicon_rorID, td_rovio, rovio_velID,rovio_rorID)
print('Calibrate Body Transform for Rovio:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
td_vicon.applyBodyTransform(vicon_posID, vicon_attID, B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist(vicon_velID, vicon_rorID, B_r_BC_est, qCB_est)
if doOkvis and bodyAlignOkvisToVicon: # Align Okvis to Vicon (body frame)
B_r_BC_est, qCB_est = td_okvis.calibrateBodyTransform(okvis_velID, okvis_rorID, td_vicon, vicon_velID,vicon_rorID)
print('Calibrate Body Transform for Okvis:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
td_okvis.applyBodyTransform(okvis_posID, okvis_attID, B_r_BC_est, qCB_est)
td_okvis.applyBodyTransformToTwist(okvis_velID, okvis_rorID, B_r_BC_est, qCB_est)
if doRovio: # Align Vicon to Rovio (Inertial frames)
td2.computeRotationalRateFromAttitude(attIDs2,rorIDs2)
# Calculate the Norm of the Rotational Rate provide ror(td1=[14,15,16], td2=[5,6,7]) and rorNorm(td1=17,td2=8) column IDs.
td1.computeNormOfColumns(rorIDs1,rorNID1)
td2.computeNormOfColumns(rorIDs2,rorNID2)
"""
We can estimate the time offset using the norm of the rotational rate.
The estimated time offset is then applied to td2.
"""
to = td2.getTimeOffset(rorNID2,td1,rorNID1)
td2.applyTimeOffset(-to)
"""
The calibration of the Body Transform needs the velocity and the rotational rate start IDs.
"""
B_r_BC_est, qCB_est = td1.calibrateBodyTransform(velBIDs1,rorIDs1,td2, velBIDs2,rorIDs2)
vCB_est = Quaternion.q_log(qCB_est)
vCB_err = vCB-vCB_est
B_r_BC_err = B_r_BC - B_r_BC_est
print('Calibrate Body Transform:')
print('Rotation Vector ln(qCB_est):\tvx:' + str(vCB_est[0]) + '\tvy:' + str(vCB_est[1]) + '\tvz:' + str(vCB_est[2]))
print('Translation Vector B_r_BC_est:\trx:' + str(B_r_BC_est[0]) + '\try:' + str(B_r_BC_est[1]) + '\trz:' + str(B_r_BC_est[2]))
print('Rotation Error ln(qCB_err):\tvx:' + str(vCB_err[0]) + '\tvy:' + str(vCB_err[1]) + '\tvz:' + str(vCB_err[2]))
print('Translation Error B_r_BC_err:\trx:' + str(B_r_BC_err[0]) + '\try:' + str(B_r_BC_err[1]) + '\trz:' + str(B_r_BC_err[2]))
"""
The calibration of the Intertial Transform needs the velocity and the rotational rate start IDs and the estimated body transform.
"""
J_r_JI_est, qIJ_est = td1.calibrateInertialTransform(posIDs1, attIDs1, td2, posIDs2, attIDs2, B_r_BC_est, qCB_est, [0,1,2])
vIJ_est = Quaternion.q_log(qIJ_est);
vIJ_err = vIJ-vIJ_est;
td_rovio.applyBodyTransformToAttCov('attCov', bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToTwist('vel', 'ror', bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyInertialTransform('pos', 'att', np.zeros(3), np.array([1.0,0,0,0]))
if doOkvis:
td_okvis.applyBodyTransform('pos', 'att', bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyBodyTransformToTwist('vel', 'ror', bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyInertialTransform('pos', 'att', np.zeros(3), np.array([1.0,0,0,0]))
if True: # Align Vicon to Rovio using calibration (body frame)
B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(Quaternion.q_inverse(qVM),bodyTransformForBetterPlotRangePos)
qCB_est = Quaternion.q_mult(bodyTransformForBetterPlotRangeAtt,Quaternion.q_inverse(qVM))
td_vicon.applyBodyTransform('pos', 'att', B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist('vel', 'ror', B_r_BC_est, qCB_est)
if doRovio and bodyAlignViconToRovio: # Align Vicon to Rovio (body frame)
B_r_BC_est, qCB_est = td_vicon.calibrateBodyTransform('vel', 'ror', td_rovio, 'vel','ror')
print('Calibrate Body Transform for Rovio:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
td_vicon.applyBodyTransform('pos', 'att', B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist('vel', 'ror', B_r_BC_est, qCB_est)
if doOkvis and bodyAlignOkvisToVicon: # Align Okvis to Vicon (body frame)
B_r_BC_est, qCB_est = td_okvis.calibrateBodyTransform('vel', 'ror', td_vicon, 'vel','ror')
print('Calibrate Body Transform for Okvis:')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
td_okvis.applyBodyTransform('pos', 'att', B_r_BC_est, qCB_est)
td_okvis.applyBodyTransformToTwist('vel', 'ror', B_r_BC_est, qCB_est)
if doRovio: # Align Vicon to Rovio (Inertial frames)
