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)