if doRovio:
plotterAtt.addDataToSubplotMultiple(td_rovio, rovio_attID, [1,2,3,4], ['r','r','r','r'], ['','','',''])
plotterAtt.addDataToSubplotMultiple(td_vicon, vicon_attID, [1,2,3,4], ['b','b','b','b'], ['','','',''])
if doOkvis:
plotterAtt.addDataToSubplotMultiple(td_okvis, okvis_attID, [1,2,3,4], ['g','g','g','g'], ['','','',''])
if plotRor: # Rotational rate plotting
if doRovio:
plotterRor.addDataToSubplotMultiple(td_rovio, rovio_rorID, [1,2,3], ['r','r','r'], ['','',''])
plotterRor.addDataToSubplotMultiple(td_vicon, vicon_rorID, [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterRor.addDataToSubplotMultiple(td_okvis, okvis_rorID, [1,2,3], ['g','g','g'], ['','',''])
if plotYpr: # Yaw-pitch-roll plotting
if doRovio:
td_rovio.quaternionToYpr(rovio_attID,rovio_yprID)
td_rovio.quaternionToYprCov(rovio_attID,rovio_attCovID,rovio_yprCovID)
td_rovio.computeSigmaBounds(rovio_yprID,[rovio_yprCovID[0],rovio_yprCovID[4],rovio_yprCovID[8]],rovio_yprSpID,rovio_yprSmID,3)
plotterYpr.addDataToSubplotMultiple(td_rovio, rovio_yprID, [1,2,3], ['r','r','r'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, rovio_yprSmID, [1,2,3], ['r--','r--','r--'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, rovio_yprSpID, [1,2,3], ['r--','r--','r--'], ['','',''])
td_vicon.quaternionToYpr(vicon_attID,vicon_yprID)
plotterYpr.addDataToSubplotMultiple(td_vicon, vicon_yprID, [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
td_okvis.quaternionToYpr(okvis_attID,okvis_yprID)
plotterYpr.addDataToSubplotMultiple(td_okvis, okvis_yprID, [1,2,3], ['g','g','g'], ['','',''])
if plotFea: # Plotting rotational rate norm
if doRovio:
figure()
x = [[]]
['g', 'g', 'g', 'g'],
['', '', '', ''])
if plotRor: # Rotational rate plotting
if doRovio:
plotterRor.addDataToSubplotMultiple(td_rovio, 'ror', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterRor.addDataToSubplotMultiple(td_vicon, 'ror', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if doOkvis:
plotterRor.addDataToSubplotMultiple(td_okvis, 'ror', [1, 2, 3],
['g', 'g', 'g'], ['', '', ''])
if plotYpr: # Yaw-pitch-roll plotting
if doRovio:
td_rovio.quaternionToYpr('att', 'ypr')
td_rovio.quaternionToYprCov('att', 'attCov', 'yprCov')
td_rovio.computeSigmaBounds('ypr', 'yprCov', 'yprSp', 'yprSm', 3)
plotterYpr.addDataToSubplotMultiple(td_rovio, 'ypr', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSm', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSp', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
td_vicon.quaternionToYpr('att', 'ypr')
plotterYpr.addDataToSubplotMultiple(td_vicon, 'ypr', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if doOkvis:
td_okvis.quaternionToYpr('att', 'ypr')
if doRovio:
plotterAtt.addDataToSubplotMultiple(td_rovio, 'att', [1,2,3,4], ['r','r','r','r'], ['','','',''])
plotterAtt.addDataToSubplotMultiple(td_vicon, 'att', [1,2,3,4], ['b','b','b','b'], ['','','',''])
if doOkvis:
plotterAtt.addDataToSubplotMultiple(td_okvis, 'att', [1,2,3,4], ['g','g','g','g'], ['','','',''])
if plotRor: # Rotational rate plotting
if doRovio:
plotterRor.addDataToSubplotMultiple(td_rovio, 'ror', [1,2,3], ['r','r','r'], ['','',''])
plotterRor.addDataToSubplotMultiple(td_vicon, 'ror', [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
plotterRor.addDataToSubplotMultiple(td_okvis, 'ror', [1,2,3], ['g','g','g'], ['','',''])
if plotYpr: # Yaw-pitch-roll plotting
if doRovio:
td_rovio.quaternionToYpr('att','ypr')
td_rovio.quaternionToYprCov('att','attCov','yprCov')
td_rovio.computeSigmaBounds('ypr','yprCov','yprSp','yprSm',3)
plotterYpr.addDataToSubplotMultiple(td_rovio, 'ypr', [1,2,3], ['r','r','r'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSp', [1,2,3], ['r--','r--','r--'], ['','',''])
td_vicon.quaternionToYpr('att','ypr')
plotterYpr.addDataToSubplotMultiple(td_vicon, 'ypr', [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
td_okvis.quaternionToYpr('att','ypr')
plotterYpr.addDataToSubplotMultiple(td_okvis, 'ypr', [1,2,3], ['g','g','g'], ['','',''])
if plotExt: # Extrinsics Plotting
if doRovio:
td_rovio.computeSigmaBounds('extPos','extPosCov','extPosSp','extPosSm',3)
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPosSm', [1,2,3], ['r--','r--','r--'], ['','',''])
