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)
plotterPos.addDataToSubplotMultiple(td_rovio, rovio_posID, [1,2,3], ['r','r','r'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, rovio_posSmID, [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, rovio_posSpID, [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_vicon, vicon_posID, [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterPos.addDataToSubplotMultiple(td_okvis, okvis_posID, [1,2,3], ['g','g','g'], ['','',''])
if plotVel: # Velocity plotting
if doRovio:
plotterVel.addDataToSubplotMultiple(td_rovio, rovio_velID, [1,2,3], ['r','r','r'], ['','',''])
plotterVel.addDataToSubplotMultiple(td_vicon, vicon_velID, [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterVel.addDataToSubplotMultiple(td_okvis, okvis_velID, [1,2,3], ['g','g','g'], ['','',''])
if plotAtt: # Attitude plotting
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)
plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if doOkvis:
plotterPos.addDataToSubplotMultiple(td_okvis, 'pos', [1, 2, 3],
['g', 'g', 'g'], ['', '', ''])
if plotVel: # Velocity plotting
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)
plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1,2,3], ['r','r','r'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterPos.addDataToSubplotMultiple(td_okvis, 'pos', [1,2,3], ['g','g','g'], ['','',''])
if plotVel: # Velocity plotting
if doRovio:
plotterVel.addDataToSubplotMultiple(td_rovio, 'vel', [1,2,3], ['r','r','r'], ['','',''])
plotterVel.addDataToSubplotMultiple(td_vicon, 'vel', [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterVel.addDataToSubplotMultiple(td_okvis, 'vel', [1,2,3], ['g','g','g'], ['','',''])
if plotAtt: # Attitude plotting
