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)