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)
J_r_JI_est, qIJ_est = td_vicon.calibrateInertialTransform(
'pos', 'att', td_rovio, 'pos', 'att', np.array([0.0, 0.0, 0.0]),
np.array([1.0, 0.0, 0.0, 0.0]), [0, 1, 2, 3, 4, 5])
print('Calibrate Inertial Transform for Rovio:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' +
str(qIJ_est[1]) + '\ty:' + str(qIJ_est[2]) + '\tz:' +
str(qIJ_est[3]))
print('Translation Vector J_r_JI_est:\tx:' + str(J_r_JI_est[0]) + '\ty:' +
str(J_r_JI_est[1]) + '\tz:' + str(J_r_JI_est[2]))
td_vicon.applyInertialTransform('pos', 'att', J_r_JI_est, qIJ_est)
if doOkvis: # Align Okvis to Vicon (Inertial frames)
J_r_JI_est, qIJ_est = td_okvis.calibrateInertialTransform(
'pos', 'att', td_vicon, 'pos', 'att', np.array([0.0, 0.0, 0.0]),
np.array([1.0, 0.0, 0.0, 0.0]), [0, 1, 2, 3, 4, 5, 6])
print('Calibrate Inertial Transform for Okvis:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' +
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)
J_r_JI_est, qIJ_est = td_vicon.calibrateInertialTransform(vicon_posID, vicon_attID, td_rovio, rovio_posID,rovio_attID, np.array([0.0,0.0,0.0]), np.array([1.0,0.0,0.0,0.0]), [0,1,2,3,4,5])
print('Calibrate Inertial Transform for Rovio:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' + str(qIJ_est[1]) + '\ty:' + str(qIJ_est[2]) + '\tz:' + str(qIJ_est[3]))
print('Translation Vector J_r_JI_est:\tx:' + str(J_r_JI_est[0]) + '\ty:' + str(J_r_JI_est[1]) + '\tz:' + str(J_r_JI_est[2]))
td_vicon.applyInertialTransform(vicon_posID, vicon_attID,J_r_JI_est,qIJ_est)
if doOkvis: # Align Okvis to Vicon (Inertial frames)
J_r_JI_est, qIJ_est = td_okvis.calibrateInertialTransform(okvis_posID, okvis_attID, td_vicon, vicon_posID,vicon_attID, np.array([0.0,0.0,0.0]), np.array([1.0,0.0,0.0,0.0]), [0,1,2,3,4,5])
print('Calibrate Inertial Transform for Okvis:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' + str(qIJ_est[1]) + '\ty:' + str(qIJ_est[2]) + '\tz:' + str(qIJ_est[3]))
print('Translation Vector J_r_JI_est:\tx:' + str(J_r_JI_est[0]) + '\ty:' + str(J_r_JI_est[1]) + '\tz:' + str(J_r_JI_est[2]))
td_okvis.applyInertialTransform(okvis_posID, okvis_attID,J_r_JI_est,qIJ_est)
if plotPos: # Position plotting
if doRovio:
td_rovio.computeSigmaBounds(rovio_posID,[rovio_posCovID[0],rovio_posCovID[4],rovio_posCovID[8]],rovio_posSpID,rovio_posSmID,3)
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;
J_r_JI_err = J_r_JI - J_r_JI_est;
print('Calibrate Inertial Transform:')
print('Rotation Vector ln(qIJ_est):\tvx:' + str(vIJ_est[0]) + '\tvy:' + str(vIJ_est[1]) + '\tvz:' + str(vIJ_est[2]))
print('Translation Vector J_r_JI_est:\trx:' + str(J_r_JI_est[0]) + '\try:' + str(J_r_JI_est[1]) + '\trz:' + str(J_r_JI_est[2]))
print('Rotation Error ln(qIJ_err):\tvx:' + str(vIJ_err[0]) + '\tvy:' + str(vIJ_err[1]) + '\tvz:' + str(vIJ_err[2]))
print('Translation Error J_r_JI_err:\trx:' + str(J_r_JI_err[0]) + '\try:' + str(J_r_JI_err[1]) + '\trz:' + str(J_r_JI_err[2]))
# Add calibrated x to plot
td1.applyBodyTransform(posIDs1, attIDs1, B_r_BC_est, qCB_est)
td1.applyInertialTransform(posIDs1, attIDs1,J_r_JI_est,qIJ_est)
plotter1.addDataToSubplot(td1, posIDs1[0], 3, 'r', 'td1Cal x');
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)
J_r_JI_est, qIJ_est = td_vicon.calibrateInertialTransform('pos', 'att', td_rovio, 'pos','att', np.array([0.0,0.0,0.0]), np.array([1.0,0.0,0.0,0.0]), [0,1,2,3,4,5])
print('Calibrate Inertial Transform for Rovio:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' + str(qIJ_est[1]) + '\ty:' + str(qIJ_est[2]) + '\tz:' + str(qIJ_est[3]))
print('Translation Vector J_r_JI_est:\tx:' + str(J_r_JI_est[0]) + '\ty:' + str(J_r_JI_est[1]) + '\tz:' + str(J_r_JI_est[2]))
td_vicon.applyInertialTransform('pos', 'att',J_r_JI_est,qIJ_est)
if doOkvis: # Align Okvis to Vicon (Inertial frames)
J_r_JI_est, qIJ_est = td_okvis.calibrateInertialTransform('pos', 'att', td_vicon, 'pos','att', np.array([0.0,0.0,0.0]), np.array([1.0,0.0,0.0,0.0]), [0,1,2,3,4,5,6])
print('Calibrate Inertial Transform for Okvis:')
print('uaternion Rotation qIJ_est:\tw:' + str(qIJ_est[0]) + '\tx:' + str(qIJ_est[1]) + '\ty:' + str(qIJ_est[2]) + '\tz:' + str(qIJ_est[3]))
print('Translation Vector J_r_JI_est:\tx:' + str(J_r_JI_est[0]) + '\ty:' + str(J_r_JI_est[1]) + '\tz:' + str(J_r_JI_est[2]))
td_okvis.applyInertialTransform('pos', 'att',J_r_JI_est,qIJ_est)
if plotPos: # Position plotting
if doRovio:
td_rovio.computeSigmaBounds('pos','posCov','posSp','posSm',3)
